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ABSTRACT 


The purpose of this thesis was to integrate a wall following motion mode for a 
rigid body autonomous vehicle. Yamabico, an autonomous vehicle located at the Naval 
Postgraduate School, was used as the test and evaluation platform. 

To implement the new motion mode, the vehicle was required to follow a straight 
wall with minor variations, navigate around comers, and avoid obstacles in its path while 
maintaining a specified offset distance from continuously connected wall segments. 
Sonar transmitter/receiver pairs were used to sense the environment and collect positional 
data for analysis. Modifications to pre-existing motion and sensor software libraries on 
board Yamabico were performed to achieve the motion goals. One of the major 
contributions from these modifications was the addition of a linear fitting algorithm using 
a decay factor. The algorithm produced quick response by the vehicle to changing 
conditions in its environment. 

The experimental results by Yamabico were successful with the algorithm 
developed by the author. The result of this thesis is that an autonomous vehicle can be 
given the capability to perform smooth and efficient motion adjustments to an 
environment composed of orthogonal wall segments and obstacles. 
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I. 


INTRODUCTION 


A. BACKGROUND 

All biological beings, from humans to insects, share the ability to move about in 
their world without constantly bumping into obstacles along their path of movement. For 
insects and less sophisticated animals, this ability is simply a sense and react type of 
behavior. More sophisticated species, including humans, are able to sense their 
surroundings with multiple sensors, process and correlate the inputs, and perform 
complex analyses to determine future movements. 

Robotics and artificial intelligence research tries to emulate these behaviors. On 
the surface, this might appear to be a relatively straightforward task. In reality, even the 
simplest behaviors can encompass an enormous amount of complexity, and emulating 
animal behavior in an artificial system is extremely difficult. This fact is evident when 
considering that this research area has been around for many decades while yielding few 
major robotics breakthroughs in natural movement abilities. This is not to say that there 
has been no significant progress. On the contrary, there have been many important 
contributions by this research area such as smart weapons, unmanned aerial vehicles, and 
robots sent to explore solar system planets, to name a few. These examples indicate that 
robotics and artificial intelligence is an important and vital area of research, but at the 
same time they show that implementing even simple functional behavior, relative to 
human standards, takes years to research and develop into actual working models. 
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The examples mentioned in the previous paragraph are considered to have simple 
functional behavior because they are either preprogrammed to perform specific tasks or 
are remotely controlled by an operator. More sophisticated models must be able to 
perform autonomously. To do this, an artificial entity must be able to analyze sensory 
inputs and make decisions based on that analysis without outside help. This process is 
something humans do easily and, for the most part, take for granted. On the other hand, 
implementing this process in hardware and software requires much effort. Dr. Yutaka 
Kanayama of the Naval Postgraduate School has been working on this problem for the 
better part of twenty years. In particular, he has created, maintained, and continuously 
modified a library of movement and sensory algorithms and data structures called the 
Model-based Mobile robot Language (MML). The robot Yamabico (Figure 1) has been 
the testing and implementation platform for experimentation with and analysis of MML. 

B. OVERVIEW 

This thesis is an extension of Dr. Kanayama's work on the Yamabico robot and 
the MML software library. Yamabico is an autonomous robot capable of using its sensor 
inputs and controlling its motion based on those inputs. This functionality is provided by 
programming code written in the C programming language and contained in the 
numerous files of the MML software library. Yamabico is also capable of following a 
predefined path using path tracking and rotational motion modes. 
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Figure 1. The Autonomous Vehicle Yamabico 11. 
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In addition to movement capabilities, MML also provides the facility to collect 
and perform analysis on sensor data. The primary sensor for Yamabico is sonar. Sonar 
data may be collected by one or more of the twelve sonars arrayed around the perimeter 
of Yamabico. Each sonar, when enabled, collects data including the distance of the sonar 
return, the robot’s position and orientation at the time of sensing, and the global position 
of the sonar return. The main analysis technique offered by the sonar system is linear 
fitting of the collected data points using the least-squares algorithm. Each sonar is 
capable of maintaining a line representation of its data collection. This ability is 
important when used for representing objects in the robot's world and maintaining safe 
distances or defined offsets from those objects. 

The tasks Yamabico performs during its run are specified in a user file, also 
written in C. This file is read in and executed following initialization of the robot. It 
provides the interface between the robot and the outside world. In this file, the 
programmer may provide the ability to set initial parameters at run time, specify a path to 
follow, and/or stipulate how Yamabico should react to particular input events. This is 
accomplished by using the input and output routines of C and by calling the functions 
contained in MML. 

Yamabico and MML provide extensive and fairly robust motion planning, 
sensing, and analysis capabilities. These capabilities can be used to direct the robot to 
perform many different tasks. However, there is currently no inherent ability for the robot 
to follow a wall or avoid obstacles automatically. These tasks could be programmed into 
Yamabico via the aforementioned user file, but this would be inefficient. Since the user 
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file is a type of interface between the outside world and the inner workings of the robot, a 
level of abstraction is introduced. This means more overhead in the program and 
increased timing delays between the software and the hardware. These factors can cause 
significant problems in the robot’s performance. It is possible to overcome these 
problems by careful and exhaustive planning and programming, but it would be much 
more efficient to give Yamabico the ability to perform these tasks at a lower level (i.e., in 
the MML software.) These lower level functionalities can be performed by an additional 
motion mode contained within MML. Implementing this new motion mode in the MML 
software is the focus of this thesis. 

C. PROBLEM STATEMENT 

The problem we want to solve in this thesis is to give the robot the capability to 
make a complete circuit around a room or rooms composed of continuously connected 
orthogonal wall segments while avoiding obstacles in its path. 

To solve this problem, a new wall following motion mode needed to be created. 

The first step in realizing this goal was to get the robot to simply follow a straight 
wall while maintaining a defined offset. The wall was allowed to have minor variations 
in its "straightness", requiring the robot to make adjustments to its path as it proceeded 
along the wall. 

After this first step was completed, the second step was adding the ability to 
negotiate orthogonal (90°) turns. To attain this goal, the robot had to handle two types of 
turns: (1) turns into the robot's current path (concave comers), and (2) turns away from its 
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path (convex comers). Once this more complex functionality was implemented, all the 
tools needed by the robot to complete a circuit around a room were in place. 

The capability to negotiate obstacles in the robot’s path was a side effect of 
solving the orthogonal wall following problem. Since obstacles in the motion path are in 
close proximity to the wall being followed, avoiding them was a natural extension of the 
robot's ability to turn at a wall intersection. By solving the orthogonal turn problem, the 
requirements to avoid obstacles was also met. 

D. ORGANIZATION 

Chapter II discusses the hardware characteristics of Yamabico. These include the 
motion monitoring and control system and the sonar control system. 

Chapter in focuses on the software library. The approach used in the software to 
model physical parameters such as position and movement is somewhat different than 
what is considered the normal representation. An explanation of the primary 
characteristics of this model is contained in the first part of the chapter. The second part 
of the chapter discusses the components of MML relevant to this thesis including the 
currently implemented motion modes and the linear fitting algorithm. 

Chapter IV begins the discussion on thesis specific topics. The programming 
code related to motion modes is examined in more detail and the modifications required 
to implement a new wall following mode is explained. A variation of the linear fitting 
algorithm is discussed and the implication of using such a variation in creating this new 
motion mode is explained. 


6 



Chapter V talks about the results of the experiments with Yamabico and the 
conclusions reached after the successful implementation of the new motion mode was 
completed. 
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II. YAMABICO HARDWARE 

All specification data in this chapter was extracted from Reference [1] unless 
otherwise noted. 

A. GENERAL OVERVIEW 

The Yamabico robot, formally Yamabico-11, is an autonomous robot composed 
of several subsystems. The robot stands 90 cm tall with a 50 cm by 50 cm base. The 
power is supplied by two 12-volt rechargeable batteries that drive the motors, sonars, and 
processor boards. Yamabico can also be connected to an external power source when 
stationary to conserve battery power. 

A PowerBook™ 145 laptop computer is mounted on top of the robot and is used 
as a mediator between the outside user, the user program and MML software, and the 
hardware. All code development and compilation is done on a Sun SPARCstation 10 
workstation using the UNIX operating system. 

Once a program is compiled, the executable is downloaded through an ethemet 
connection to a SPARC IV main CPU board on a VME bus. The laptop then runs the 
program which initializes Yamabico's hardware, executes the user progr am^ and handles 
the hardware interrupts. After Yamabico has completed its run, any data earmarked for 
collection via MML logging functions can be downloaded from the laptop to the UNIX 
network through a standard phone line cable. 

There are four major subsystems in Yamabico that interact asynchronously to give 
it its capabilities. The motion and sonar subsystems are the first two and are the focus of 
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the remainder of this chapter. The software library and user program comprise the third 
subsystem. They are discussed in Chapter HI. Finally, Yamabico has an optical image 
processing subsystem, but this subsystem was not used for this thesis and will not be 
discussed. 

B. MOTION MONITORING AND CONTROL SYSTEM 

Yamabico is a wheeled differential drive vehicle. Two separate 35 Watt DC 

motors drive each wheel via drive shafts using a 1:24 gear box ratio. The drive shafts are 
mounted with optical encoders to enable the robot to calculate how far each wheel has 
traveled in a given time step. This information is used to make odometry calculations. 
Four shock absorbing caster wheels are also mounted on the base of Yamabico to provide 
stability. Since this is an indoor vehicle, there was less emphasis on detailing physical 
factors such as slippage and uneven terrain effects and the more general approach of dead 
reckoning (DR) was used in movement calculations. 

The DR approach does cause navigational problems over long running times. For 
instance, if the floor is extremely slippery and a high acceleration is used, the wheels may 
slip, resulting in a measured travel distance that is larger than the actual true distance of 
travel. As another example, if the robot drives over a board or an uneven floor segment, 
the robot may measure a turn when no turn was made. These errors are minor and 
acceptable over small distances. When the robot moves over much larger distances, the 
errors compound until eventually they are no longer negligible. At this point adjustments 
have to be made by the robot to realign itself with the real world. Several different 
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techniques can be used to update the robot's navigation. In a known or partially known 
world, a landmark can be used to correct navigational errors. Another technique, and the 
one used in this thesis, is to navigate by continuously updating its position and future path 
in reference to the world (i.e. follow a wall or walls.) This is explored in detail in 
Chapter IV. 

The motion monitoring system interrupts the software system on regular 10 ms 
intervals. Yamabico's nominal speed is 30 cm/sec with a maximum speed of 65 cm/sec. 
At the nominal speed, interrupts occur every 0.3 cm as indicated by Equation (2.1). 


30—xlO' 2 


sec _ q 2 cm 

interrupt interrupt 


This short travel distance between motion updates provides a sufficiently high 
degree of accuracy in odometry calculations. During each interrupt, information from the 
drive shaft encoders is fed to a software interrupt routine. The software can then use this 
information to calculate its new position, make adjustments to future movement 
requirements, and return commanded linear and rotational velocities to the motion 
processor board. The board then translates these commanded velocities into electronic 
signals to the two wheel motors in a pulse-width modulation mode to effect the requested 
movement. 


C. SONAR AND SONAR CONTROL SYSTEM 

Yamabico s sonar system consists of twelve sonars arranged in a horizontal plane 
approximately 30 cm above ground level and angularly spaced in 30° increments to give 
full 360° coverage (see Figure 2). Each sonar is a transmitter/receiver pair operating at a 
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frequency of 40 kHz and may be individually enabled or disabled at any time within the 
user program. 
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Figure 2. Sonar Configuration of Yamabico After Ref. [1]. 

The sonars are controlled by a separate processing board on Yamabico. In order 
to reduce sampling time, the sonars are organized into 3 groups of 4 sonars apiece. Each 
group contains sonars that are separated by 90° to minimize interference from 
neighboring sonar transmissions. All enabled sonars within the same group are pulsed 
simultaneously thereby reducing the time spent sampling by a factor of four when all 
twelve sonars are in use. The sonar groupings are shown in Table 1. 
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Group 

Sonars in Group 

0 

0,2,5,7 

1 

1,3,4, 6 

2 

8,9,10,11 


Table 1. Sonar Groupings. 


As in the motion control hardware, the sonar system generates sonar interrupt 
events in the software. These interrupts occur at 50 ms intervals. Because the sonars are 
activated in groups, the time between pings of a single sonar can take up to 150 ms . 
When one or more sonars in a group are enabled, that group generates an interrupt and 
each enabled sonar in the group is activated. Similarly, if no sonars in a group are 
enabled, that group does not generate an interrupt. With these facts in minH the time 
between pings on an single enabled sonar has a minimum value of 50 ms and a maximum 
value of 150 ms. This design is much more efficient than activating sonars individually, 
which could result in a maximum of600 ms between pings. 

In the original design of the sonar system, a pulse width of 1 ms was used. This 
was later changed to the current value of 0.5 ms to reduce the minimum range of the 
sonars. Assuming the speed of sound in air at room temperature is approximately 340 
m/s, the minimum range can be calculated as half the distance the sound will travel in the 
time of a transmit pulse (pulse width). 

minrange=H 34000—x 0.5 ms) =8.5 cm (22) 

2\ sec J v ' J 
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The system actually sets the minimum range to be 9.6 cm to allow for circuit 
switching and settling times. 

The maximum range of the sonars is imposed by hardware limitations. When a 
pulse is sent out, a 16 bit data register is used to count time until the pulse is received. 
Only the first 12 bits of this register are used for counting, limiting the counter to a 
maximum value of4095 (2 12 -1). The counter is incremented every 6 ps which 
corresponds to an incremental distance of 0.204 cm. The maximum range of the sonar is 
thus: 

max range=—(4095 x 0.204 cm)=417.7 cm (2.3) 

Again the system allows for circuitry timing and various other long range errors 
and sets the maximum range to be 409 cm. 

The location of each sonar was depicted graphically in Figure 2. In order 
calculate global positions of the sonar returns, the software system maintains a table, 
similar to Table 2, of sonar positions relative to the center of the robot with the x-axis 
aligned to the front of the robot and angles measured counter-clockwise from the x-axis. 
Global positions are then calculated using the robot's current position, the relative 
position of the sonar making the measurement, and the distance returned by the sonar. 
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| Sonar 

X (cm) 

Y (cm) 

6 (rad) 


23.6 

-0.5 

0 

i 

-23.0 

13.1 

5n/6 

2 

-22.6 

-1.0 

71 

3 

24.7 

-14.6 

-n/6 

4 

13.4 

21.3 

7t/3 

5 

0.0 

20.6 

n/2 

6 

-12.6 

-21.3 

-2n/3 

7 

0.0 

-20.5 

-n/2 

8 

-13.4 

21.3 

2n/3 

9 

-23.5 

-14.9 

-5n/6 

10 

12.1 

-21.3 

-n/3 

11 

25.2 

14.1 

n/6 


Table 2 


Sonar Positions Relative to Center of Robot. 

















































III. SOFTWARE LIBRARY 

The software system used to control Yamabico is both complex and robust. 
Interrupts are used to optimize operations. Timing considerations of these interrupts are 
an important part of code development. The MML software library is composed of many 
files each containing software functions written in the C progr amming language. 

Multiple files are used to separate and organize the main objectives of each part of the 
system. These objectives can be roughly categorized as low level motion control, sonar 
control, and sonar analysis, and high level geometry, input/output, and user control. 

A. INTERRUPT CONTROL SYSTEM 

As mentioned in Chapter n, Yamabico operates in an interrupt based 
environment. The software library serves as a multi-tasking operating system for the 
robot [1]. It manages interrupt requests based on the relative importance of the task 
associated with each type of interrupt. When received, higher priority interrupts are 
handled immediately and lower priority tasks are suspended until completion of the 
corresponding interrupt handling routine. 

The software system designates eight levels (0 - 7) of interrupt priorities. Table 3 
depicts these levels with higher level numbers indicating higher priority. The interrupt 
type indicates how each interrupt level interacts with the rest of the system. 

Asynchronous interrupt events are processed immediately while synchronous events, as 
the name implies, are synchronized with circuitry timing 
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Interrupt Level 

Interrupt Source 

Function 

Interrupt Type 

7 

stop button 

reset 

asynchronous 

6 

— 

not used 


5 

— 

not used 

— 

4 

Serial Board 1 

locomotion 

synchronous 

3 

Serial Board 2 

teletype 

asynchronous 

2 

Sonar Board 

sonar 

synchronous 

1 

Serial Board 0 

debugger 

synchronous 

I 

0 


user instructions 

none 


Table 3. Interrupt Priority Table for Yamabico After Ref. 
[ 1 ] • 


A reset button is installed on top of Yamabico where it is readily available to the 
operator. When depressed, this button generates an interrupt of the highest level (level 7). 
It causes the robot to halt immediately and resets all hardware components. This button 
is used extensively during development of new routines for Yamabico because invariably 
these new routines have flaws in logic or unexpected side effects. By using the reset 
button, the operator may cease Yamabico's movement before a collision occurs. 

Interrupt levels 5 and 6 are currently unused. 

The motion system is incorporated at interrupt level 4. This is the highest priority 
interrupt that occurs during normal operation of Yamabico. Since motion hardware 
interrupts occur every 10 ms, this priority level reflects the importance of handling this 
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type of event immediately. All steering and odometry functions are run during the 
processing of this type interrupt. 

All console operations of the laptop and data transfer between the laptop and the 
UNIX network have interrupt priority level 3. This allows for feedback to the outside 
user, input from the user, code uploading, and data downloading. 

The sonar system operates at interrupt level 2 allowing sensor updating to occur at 
a relatively high priority. If no sonars are enabled, this level is bypassed during the 
interrupt polling procedure. 

At the lowest levels are debugging routines at level 1 and the user control 
instructions at level 0. All other interrupts can override this last level. In general, level 0 
can be thought of simply as code that runs whenever the system has time to run it. This 
last statement may seem misleading because the robot won't really do anything without 
the user code. It is in this part of the software that the initial state of the robot is set (i.e., 
position, initial speed, path, enabled sonars, data logging, etc.) and end of run conditions 
are checked. However, since these functions can be considered as initialization routines 
(before motion and sensing starts) or short status checks (during run), there are no adverse 
effects of placing them at the lowest interrupt level. 

B. TRANSFORMATION MODEL OF MOTION 

Before delving into the details of the motion algorithms in MML, an 
understanding of the underlying mathematical model must be gained. In most physical 
applications, Cartesian or polar coordinates are use to model kinematic effects. Dr. 
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Kanayama has developed a related but somewhat different and more versatile model of 
motion of a rigid body robot. This model is based on the fact that the trajectory of a rigid 
body object can be represented by a sequence of points, and since we calculate the 
vehicle's position at discrete intervals, the positional changes can be represented by a 
series of discrete transformations. These transformations are the heart of the motion 
model. This section presents a synopsis of Dr. Kanayama's transformation model of 
motion [2,3,4, 5,6]. 

1. Transformations and Configurations 

A transformation is described by three parameters, the conventional two- 
dimensional Cartesian coordinates (x, y) and an orientation angle (0). In a rigid body 
vehicle, these correspond to a translation and a rotation (or heading change). Note that in 
this model, positive angles are measured in the counter-clockwise direction relative to a 
reference (local or global) x-axis. There is also the idea of a configuration. A 
configuration describes the current state of the vehicle using the same parameters of a 
transformation but with somewhat different meanings. In a configuration, x and y 
represent the global coordinates of a local reference frame and 0 represents the current 
orientation of the local x-axis. Both transformations and configurations are specified by a 
triple (x, y, 0). The main difference between a transformation and a configuration is that 
the former acts as an operator and the latter acts as a spatial specification of an object. 
Figure 3 shows an example of an initial configuration (q 0 ) which undergoes a 
transformation (TO resulting in a new configuration (qi). 
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2. Composition 

In Figure 3, to get from q 0 to q h the transformation Ti is applied to the 

configuration qo by a method called composition represented by the symbol (°). 

Composition is defined as follows: 

l&t q o {xq ,y q,Q and T\ 

Then: 
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(3.1) 


r x 0 +x, cos# 0 -y x sin0 o 

^o+XjSin^+^cos^o 

^ 0 O +0j 

In this thesis, the two forms seen above (i.e. parameter triple and column vector) 
will be used interchangeably to represent a transformation/configuration. 

3. Inverse and Relative Transformation 

In Figure 3, q x = q 0 ° 7]. In order to get from qi back to qo, an inverse operation 

is needed such that q x © 7j _1 = o 7] © 7]" 1 = # 0 , where 7J' 1 is the inverse transformation 

of T x . When a transformation is composed with its inverse, it produces an identity 

transformation which neutralizes its effect in an expression such as that above. The 

definition of an inverse transformation is: 

LetT = (x,y,0), 

Them. 

-xcosO-ysinO' 

xsinO-ycosO (3.2) 

-e 

One use for the inverse function is in finding relative transformations. Suppose a 
robot vehicle is represented by the global configuration qv, and an object is represented 
by the global configuration qo, as shown in Figure 4. To find the object's relative 
transformation from the vehicle, there must be a transformation q* such that q v ° q* = qo* 
Composing both sides with the inverse of qv results in q* = (qv)' 1 © qo, the 
transformation of the object relative to the vehicle's coordinate frame. 
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Figure 4. Example of a Relative Transformation (q*). 

4. Lines, Circles, and Paths 

For path planning, the transformation/configuration model is expanded so that 
lines and circular arcs can also be expressed as configurations. For this purpose, a 
configuration is redefined by adding a fourth parameter. This fourth parameter (k) 
represents the curvature of the configuration where 1/k is the radius of curvature. Lines 
become directed lines with the x and y parameters specifying its origin, 0 its orientation, 
and since the radius of curvature of a line is infinite, k = 0. For example, a line (L) 
beginning at the global position (x = 1, y = 5) and angled 45° above the x-axis would be 
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defined by the configuration L — ^1,5,-j- ,0J. Circles are somewhat more complex, but 

follow similar reasoning. A circle configuration is a directed circle starting at the (x, y) 
position (on the circle), oriented in the 0 direction (tangent to the circle), with curvature 
k — 1 /radius. For example, one representation of a counter-clockwise circle centered 



Figure 5. Example Line(L) and Circle(C) Configurations 
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These ideas of transformations and configurations are a very powerful and can be 
applied in a variety of ways. For instance, to specify a simple square pattern starting at 
the origin, only one configuration and one transformation is needed. The following 
algorithm will produce four lines making the square shown in Figure 6: 




Figure 6. Square path pattern defined by an initial 
configuration and successively applying a transformation. 

Path planning for Yamabico is accomplished in this manner, where a path is 
specified as a sequence of lines and circular arcs. 
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5. Circular Transformation 

So far only large scale transformations have been depicted. Yamabico updates its 
odometry every 10 ms. In this small time period, the distance it moves is very small 
(~0.03 cm). Taking advantage of this fact another concept, called a circular 
transformation, is used in path trajectory calculation. 

First, consider a path (P) which can be defined by an initial configuration 
$o = (*o >*o) 311(1 future configurations as a function of arc length s. The path 


is completely specified by k as a function of s. Because the curvature is the derivative of 


the tangential orientation 




along P, once k(s) is defined, 0(s) is found by: 


0(s) = 0 o + jfc(u)du (3.3) 

o 

Positions along P are also functions of s and are specified by the following 
parametric equations: 


x(^) = x 0 + jcos(0(u))du (3.4) 

0 


A. s ) = y 0 + J sin( £(«))</*/ (3.5) 

0 

These equations are a continuous representation of a path, and the instantaneous 
configuration of a vehicle following this path is: 

tf( 5 ) = (*(*)> X*)’ (3.6) 
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In digital computer based systems, continuous functions are represented by a 
series of discrete changes or deltas (A). In Yamabico, a continuous path is represented by 
a series of discrete instantaneous configurations 

(3.7) 

Each successive configuration can be found by the following iterative technique: 

(3.8) 

where the Aqj's are incremental transformations over a small path distance As. 

If As is sufficiently small, we can use the following approximation to the amount 
of change in orientation over As: 

A0 = x-(j)As (3.9) 

To improve this approximation we use die value of the curvature at the midpoint 
of the interval [s, s + As]: 

A0 = ^+-jJas (3.10) 


If it is also assumed that the curvature along the interval [s, s + As] remains 
constant, then the interval represents a circular arc of radius 1/k. Figure 7 depicts this 
circular arc with curve length As and orientation change A0 starting at the origin of a 
reference frame with initial orientation aligned with the x-axis. 

The radius of the circular arc is: 


1 As 
V ~ k~ A0 


(3.11) 
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Figure 7. Circular Arc Parameters After Ref. [2]. 


An expression for Aqj can now be formulated as a circular transformation, denoted 
by C(As, A0). First, for A0 * 0: 


C(As, A#) s (Ax, Ay, AO) 

= (r sin A0,r(l - cos A#), A 0) 



1-cosAff 

AO 


As, AO 


(3.12) 


For A0 = 0, the circular arc becomes a straight line and: 

C(As,A0) = (As,O,O) (3.13) 

Finally, |A0| is normally small enough to justify using a Taylor expansion for the 
sine and cosine terms in Equation (3.12). With these expansions. Equations (3.12) and 
(3.13) can be condensed into a single equation: 
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C(As,A0) = 


(f 


A0 2 A0 4 


si 


5! 


As,- 

7 • 


1 AO 2 A0 4 \ 


2! 4! 


6 ! 


A0As,A0 


(3-14) 


Using Equation (3.14) any path can be generated using the following algorithm: 



This algorithm closely matches the algorithm Yamabico uses to calculate its 
trajectory while it is in motion. The robot must perform some extra steps such as 
calculating As by averaging the amount of rotation of each wheel and dete rmining what 
kind of path shape it is following, but the algorithm presented above is central to 
computing the vehicle's current position. 

6. Tracking Lines 

In the preceding discussions a method for specifying a desired path for the robot 
(lines and circles) and a method for calculating the robot's trajectory (circular 
transformation) have been defined. In order to have the robot match its trajectory to the 
desired path, there must be a way to determine how far the robot is away from the path 
and how to proceed to the path. Just as the driver of a car uses the steering wheel to 
match the vehicle's trajectory to the desired path (the road), the robot uses a steering 
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function to accomplish this task. The first item to look at is using the steering function to 
track a line (see Figure 8). 



To generate a smooth trajectory, the curvature function must maintain continuity. 
To fulfill this requirement, the derivative of the trajectory curvature must remain finite at 
all times and is used as the definition of the steering function J(L): 

die 

— = J(L) = -aK-b<&(0-e,)-cM (3.15) 

where a, b, and c are positive constants, O is an angular normalization function 
(normalizes angular range to the interval \-n, 7 t \), 0i is the orientation of the line (L), 

and Ad is the signed distance from the line. For Ad, positive distances are to the left of 
the directed line and negative distances to the right. 
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The three terms on the right hand side of Equation (3.15) are negative feedback 
terms and the coefficients are feedback gain parameters. The first term -aic is negative 
feedback for curvature error (ki = 0). The second term -bO is negative feedback for the 
orientation error. The third term -cAd is negative feedback for positional error (Ad is 
measured normal to L). When combined in this way, these terms allow the robot's 
trajectory to converge to the line (L) without overshoot or undershoot. However, this is 
only true for an optimal combination of the gain parameters (a, b, c) defined below: 

a = 3k 

b = 3k 2 (3.16) 

c-k 3 

where k is the gain of the steering function. See section 3.2 of Reference [5] for a 
detailed derivation of these parameters. 

The smoothness of the line tracking motion is defined by: 

cr = j (3.17) 

which represents a smoother motion with larger o and a sharper motion with 
smaller a. This parameter is the only parameter in the steering function J(L), and is used 
to control how fast the robot reacts to the feedback. 

Figure 9 illustrates the effect of different c values on the robot's trajectory when 
the robot starts at q 0 = (0,100,0,0) and tracks the x-axis, L = (0,0,0,0). 
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7. Tracking Circles 

The steering function for directed circle tracking is similar to that of Equation 
(3.15). In line tracking, the curvature of the path (L) was zero. With circle tracking, the 
curvature of the path has a value other than zero and must be taken into consideration. 
This thesis does not make extensive use of circle tracking, and therefore the definitions of 
the steering function values are stated below without detailed explanation. Figure 10 
depicts the parameters involved in circle tracking. 

The steering function for tracking a circle (L) is: 

die 

— = J(L) = -a(tc -AT,)- bd3(e - 0,) - cd' (3.18) 

UJ 
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Figure 10. General Circle Tracking From Ref. [2]. 

where Ki is the curvature of the circle, 6i is the tangential orientation of the closest 
point on the circle, and d* is the signed distance to the closest point on the. The gain 
coefficients are also effected by the curvature of L and become: 

a = 3k 

b = 3& 2 —k\ (3.19) 

c=e-3k K ] 

References [2, 6] provide an in depth discussion on these topics. 

8. Tracking Paths 

The steering function presented in the previous two sections tells us how to track a 
single directed line or directed circle. A path can be defined by a sequence of path 
segments (lines and circles), such as the square path in Figure 6. When changing between 
the current path segment and the next sequential path segment, a neutral switching point 
is defined to prevent unnecessary early turns or overshoots. This is accomplished by 
maintaining two steering function values, J(L f ) for the current path segment and J(L i+ i) 
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for the next path segment. The neutral switching point occurs when these two values are 
equal, J(Lj) = J(Li+i). Since these steering values are floating point numbers, the check is 
made in program code by monitoring the sign of J(Lj) - J(Li+i). When the sign changes, 
the neutral switching point has been reached. This approach avoids checking the 
equivalence of two floating point numbers, which can lead to errors. The algorithm 
shown below demonstrates how neutral switching is implemented in Yamabico when 
following a defined path P: 

ltf = 4o 

2. P = (L„L 2 ,...,L n ),n>\ 

3. do forever 

4. if (n > 2 and (the sign of J{L X ) - J{L 2 ) has changed)) 

5. then shift pathsegments(Z,, <—L 2 ,L 2 <-Z, 3 ,...) 

6 . k = k + J[L x )As 

7. q~q° C^AsjxAs’) 

C. THESIS RELATED MML ROUTINES 

The previous section (Section B) showed how the transformation model of motion 
can be used for path planning, trajectory calculation, and relative positioning computation 
of objects in the world of a rigid body robot. This section discusses how this model is 
used in MML to direct the motion of Yamabico. 

1. Motion Modes 

Yamabico is primarily controlled by two types of motion modes. The first of 
these is a track mode and the second a rotate mode. The track mode is further broken 
down into two types, the normal track mode in which lines and circles are tracked in 
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sequence, and the track stop mode where the current line or circle is tracked while the 
robot comes to a halt. The rotate mode is simply a turn in place type of motion. 

By using these modes, many types of paths can be constructed for and followed by 
Yamabico. The user specifies the path by issuing a sequence of track and/or rotate 
commands in the user control file. For each track or rotate command, a corresponding 
path element is inserted into an instruction buffer (or queue) to be extracted by the motion 
control routines. Figure 11 presents a block diagram of this process. 



Figure 11. Motion Control Setup After Ref. [1]. 


The process begins in the file <user.c>, included in Appendix B. The user 
defines a sequence of configurations (x, y, 0 , k) and issues a track (function track()) or a 
rotate (Rotate()) command for each configuration. The final configuration in the 
sequence is issued with a track stop (tracks ()) command so that motion will stop when 
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the sequence is complete. The track()/Rotate()/trackS() command functions are located 
in the file <seqcmd.O (Appendix A). They take each configuration and add path type 
information, steering related information, and circle information if the configuration 
specifies a circle. The resulting path element is stored in a data structure called a LINE, 
defined in <definitions.h> (Appendix A). This LINE is then inserted into the instruction 
buffer via the function AddLinef), located in <queue.O (Appendix A). 

This process does not directly control Yamabico's motion. In other words, the 
sequence of configurations is a list of directions for Yamabico to follow in the future. 

The motion control routines in <motion.O (Appendix A) are the ones that control 
Yamabico. They receive the instructions from the buffer and carry them out according to 
an algorithm like the one mentioned in Section B.8. The algorithm is actually spread out 
over several functions and files. Lines 1 and 2 of that algorithm are executed by the user 
program. The initial configuration is either specified by the user or a default value is 
used. The path P is specified by the instruction buffer built by the track and rotate 
commands and the current path element is kept in the static variable currentPath local to 
the file <motion.c>. 

The loop in lines 3-7 of the algorithm is executed once every motion control cycle 
(10 ms). When the system signals a motion interrupt, the motion interrupt handler is 
called. This function, MotionSysControlQ in <motion.c>, is the workhorse of the motion 
control system. It first updates Yamabico's movement by calculating how far each wheel 
has traveled during the last motion control cycle. It then uses this information to compute 
its path change (As) and orientation change (A0). 
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After updating a variable keeping track of the total distance the robot has traveled 
the MotionSysControlO function calls the localize^ function which uses the calculated As 
and A0 to update the vehicle's current configuration by circular transformation. The 
localization corresponds to line 7 of the algorithm. According to the algorithm, this step 
is out of order. This is due to the fact that As and A0 are computed directly from the 
hardware and can be used immediately. The ordering difference is negligible since k 
changes only a small amount during the motion control cycle. 

The next step in MotionSysControlO calculates the actual linear and rotational 
velocities maintained by the robot. These are used in later calculations. At this point all 
motion data from the last motion control cycle has been calculated and LogMotionO is 
called to log the motion data if logging has been turned on. 

MotionSysControlO next calls motionRulesO to determine the commanded linear 
and rotational velocities for the next motion control cycle. The motionRulesO function 
performs all the calculations involving future movements. It encapsulates the 
functionality of lines 4-6 in the algorithm. Depending on the motion mode (track or 
rotate), it directs program execution to an appropriate motion rule handlin g function. The 
motion rule function (trackRule() or rotateRule()) is responsible for performing stee ring 
function calculations, extracting the next path element from the buffer if appropriate, 
updating k, and returning the commanded velocities for the next motion control cycle. 

The returned commanded velocities are relayed back through the motionRules() 
function to the MotionSysControlO function where they are translated into commanded 
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velocities for each wheel by the function SetMovement(). Once this is done, the system 
clock is updated and an external LED is blinked for visual feedback. This completes one 
motion control interrupt cycle and control is returned back to the main system where 
other computations are performed or other interrupts are handled. 

2. Linear Fitting 

Each sonar on Yamabico is capable of gathering and maintaining collections of 
data in both raw form (distance from sonar) and in global form (global position of the 
return). The related data structures are contained in the file <sonar.h> (Appendix A) and 
related functions in <sonarmath.c> (Appendix A). Some of these functions provide the 
ability to fit a line to a collection of global positions using the least-squares summation 
technique. 


a) Equal Weighted Points 

The following discussion of least-squares linear regression analysis is 
extracted from Ref. [1] and follows techniques covered in most probability and statistics 
textbooks, such as Ref. [7], 

For a set of equally weighted unique points, a line that best fits the points 
can be defined as that line which minimizes the sum of the squares of the distances from 
the points to the line. The distance of a point to the line is called the residual and is 
denoted by 8. The goal is therefore to minimiz e the sum S = ^ S 2 . 

First, let R = {p l ,...,p n }, where n > 2 and pi = (xj, yO, be the set of n 
points to be fitted. The moments 1% (0 < j + k < 2) of R are defined as: 
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The centroid C of the set R is given by: 
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In finding a line L to represent a best fit, a parametric representation of L 
is adopted using the line's normal direction a and distance r from the origin O. This 
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approach is different from standard linear regression analysis but has the advantage of not 
having any singularities with respect to vertical lines. The line is therefore defined in 
terms of these parameters, L = (r, a) (see Figure 12). 



Figure 12. Parametric Representation of Linear Fitted Line 
After Ref. [2]. 

Note that every point (x, y) on the line L = (r, a) satisfies the relation: 

rcosor+y sina = r (3.30) 

which implies that the residual of the point is: 

£ = jtcosa + ysina-r (3.31) 

Also note that r is negative if the projection of 0 onto L is in the second or 
third quadrant (Figure 13). 

Finally, the sum of squares of the residuals becomes: 

S = 8 2 = ^ ((x, cosa+y, sin or) - r) 2 (3.32) 

»=i i=i 
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where atan2() is a more robust arctangent function found in most 
computer language libraries. It eliminates quadrant ambiguities retu rning an angle in the 

range [-7t, ji] and therefore, by Equation (3.35), a is restricted to the range 

The four possible quadrant outcomes for L are depicted Figure 13. 

The linear fitting routines in Yamabico use this procedure to calculate L 
for the global positions of sonar returns. Each of the twelve sonars is able to do this 
independently and stores current linear fitted data in their own data structure. When a 
new sonar return is received, the point data is used to update the linear fitted data by 
adding it to the moment summations in Equations (3.20-3.25). For instance, let (x„+i, 
yn+i) be the new point and m' jk (0 < j + k < 2) be the current value of each moment. The 



new moment summations m Jk would then be calculated as follows: 


’00 =< +1 = « + 1 

(3.36) 

m l0 = m [o +x n+1 

(3.37) 
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Figure 13. Four Quadrant Possibilities for Linear 
Fitted Line. 


m oi =K +y n+ \ 

(3.38) 

m n =m' u +x n+l y n+l 

(3.39) 

m 20 =m' 2 o+x„ 2 +] 

(3.40) 

m <>2 =<2 +yl 1 

(3.41) 


The centroid and secondary moments are then recalculated using these 
new values, followed by a recalculation of r and a yielding a new line L. When the 
distance from a new point's location to the current L exceeds a threshold value, 
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processing of the current line segment stops, its endpoints are stored, the moments are 
reset, and calculation of a new line segment is begun using the new point data as the first 
point. 

This process is repeated on every sonar control cycle for each sonar 
enabled for linear fitting. The building of line segments can be used to map a room, 
determine the shapes of objects, or update navigation by comparing the line segments to a 
map. All of these uses are passive and, except for the last one, do not contribute to the 
robot's motion analysis. 

b) Using a Decay Factor 

In order to follow a wall with sonar, a more reactive technique must be 
applied. Through conversations with Dr. Kanayama a modified version of the standard 
least-squares algorithm was developed for use in Yamabico. This modified version uses 
the idea of a decay factor applied to current linear data when adding a new point. 

In Equations (3.36-3.41) there is an implicit multiplicative factor of one 
applied to the m' jk terms. This follows from the stipulation that all points are equally 

weighted. If this factor is changed to a value less than one, then as new points are added, 
the effect of previous points on the linear data would decay in significance. In other 
words, there is still an overall history of previous data but new points have a greater 
impact on the calculation of the line L. In effect, the new point has a weight of one, the 
previous point has a weight of DF (where DF = decay factor), the point prior to that has a 
weight of (DF) 2 , etc. Rewriting Equations (3.36-3.41) to reflect this change results in: 
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m w - DF x m'w +1 = DFxn + \ 

(3.42) 

m l0 = DF x m[ 0 + x„ +1 

(3.43) 

m 0l =DFxm’ 0l +y n+l 

(3.44) 

m n =DFxm; i +x„ +t y n+1 

(3.45) 

m 20 = DF x m' 2() + x„ 2 +I 

(3.46) 

m 02 =DFxm' 02 +yl, 

(3.47) 


Each time a new point is added, the moments are recalculated in this 
manner and, as in the equal weighted procedure, the centroid, secondary moments, r, and 
a are recalculated to give a new representation for the line L. This technique yields good 
response to a changing environment. It is reactive in that instead of having to start a new 
line segment if a new point is beyond a certain threshold, as mentioned in the previous 
section, the line L is adjusted to fit the new data. In this way, the line constantly changes 
and adjusts to the environment. 

Figure 14 shows the effect of three different decay factors (< 1) compared 
to no decay factor (= 1) on the linear fitted line. It simulates a robot traversing a wall and 
taking sonar readings. The wall is represented by the connected solid line. The global 
position of the sonar return from the wall is represented by the intersection of the dashed 
vertical lines and the wall. The orientation of the linear fitted line is depicted by the solid 
line segments at the top of the dashed lines. 

Analysis of Figure 14 results in the conclusion that smaller decay factors 
provide greater response to new data. For wall following, the robot should react to 
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changes in a timely manner while maintaining a smooth and continuous path curvature. 
Decay factors that are too large (i.e. close to one) violate the first stipulation while decay 
factors that are too small violate the second stipulation. It was decided through empirical 
observation that a decay factor of 0.9 would be used in the linear fitting with decay factor 
algorithms. 
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Figure 14. Linear Fitting Decay Factor Effect. 
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Linear Fitting w/ Decay = 0.30 




















































































































IV. WALL FOLLOWING AND OBSTACLE AVOIDANCE 

The foundation for solving the problem defined for this thesis lies in having 
Yamabico maintain an offset from a wall while moving in the forward direction. The 
first attempt in realizing this goal was to call existing MML functions from the user file to 
direct Yamabico's motion. This approach proved to be flawed and it was dete rmin ed that 
implementing a new type of motion mode would be the best method for solving the 
problem. Implementation of this new motion mode required modifications to the MML 
software library. 

This chapter discusses why the first solution attempt failed, how a template of a 
new motion mode was added to MML, and how this template was filled in to solve the 
problem of following a wall. 

A. FIRST ATTEMPT 

Section C.l of Chapter HI explained how currently implemented motion modes 
are used to define a path that Yamabico will follow. For instance, if the user wanted the 
robot to follow a square path, such as the one described by Figure 6, the following code 
would be created in the <user.O file: 
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CONFIGURATION qO, L, T; 
int n; 

L = qO = defineConfig(0, 0, 0, 0); 

T = defineConfig(100, 0, PI/2, 0); 
setRobotConfiglmm(qO); 

for (n = 1; n <= 4; i++) { 

track(L); 

L = compose (&L, &T); 

} 

L = defineConfig(150, 0, 0, 0); 
tracks(L); 


In this example, CONFIGURATION is a data structure defined in <definitions.h> 
holding configuration parameters (x, y, 0 , k), setRobotConfigImm() is a function defined 
in <motion.o which immediately sets the robot's configuration, compose() is a function 
defined in <geometry.O (Appendix A) implementing the composition (o) operation, and 
track()/trackS() are the functions discussed in Chapter HI. The/or loop creates the 
square path by successively calling the track() function with the current line 
configuration L, which adds L to the instruction buffer, and then composes L with the 
transformation T to get the next line. The final two lines of code create the last line 
configuration in the sequence so that the robot will stop when the sequence is complete. 

This procedure works well for predefined paths because it sets up the instruction 
buffer before any motions, and hence motion interrupts, occur. There is another function 
defined in the file <queue.c> called FlushBuffer(), which flushes the instruction buffer. 
This function is a low level function, similar to AddLine(). 
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The first attempt at a solution for wall following tried to use these low level 
functions, FlushBuffer() and AddLine(), in the high level user program. The idea was to 
track an initial line until sufficient linear fitted data on sonar returns from the wall was 
acquired. Once the robot had good wall data, it would construct a line configuration from 
the linear data, flush the buffer, and track the new line by adding it to the instruction 
buffer. This solution idea was flawed because of timing differences between the motion 
interrupt cycle, the sonar interrupt cycle, and the user program. The low level instruction 
buffer routines were not intended to be used in the high level user routines. T his fact was 
not originally known to the author, but was realized after careful analysis of the code. 

The problem was that, once robot movement commenced, the motion interrupt 
routine had a higher priority than the sonar interrupt routine which, in turn, had higher 
priority than the user program (Table 3). The only possible way to fix this problem was 
to disable interrupts (normally bad idea in interrupt driven programs) during critical 
sections of code in the user program. However, this still caused problems. If a disable 
interrupt command was given in the user code, there was no guarantee that the command 
would be carried out immediately because this command was contained in a low priority 
section of the code. These timing problems were too much to overcome for solving a 
quick reaction type of problem and this first solution attempt was discarded. 

B. IMPLEMENTING A NEW MOTION MODE 

To implement a reactive type of motion mode in MML, the use of an instruction 
buffer must be bypassed. The functionality of this new motion mode should closely 
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mirror that of the tracking motion mode, but should also be self-sufficient without 
holding path elements in a buffer. In other words, the user program should be able to call 
a routine which initializes the system to perform the tasks of the new motion mode, but 
the tasks should be performed by the lower level functions. 

In the tracking motion mode, the user calls track()/trackS() (in <seqcmd.c>) to 
initialize the system by building the instruction buffer. After the initialization is 
complete, the motion interrupt handling routine, MotionSysControl() in <motion.c>, 
calculates commanded velocities for the robot on each interrupt cycle by calling 
motionRules(). This function determines the current type of motion mode (e.g. 
TRACKMODE) and calls the appropriate motion rule function, such as trackRule(). The 
motion rule function handles the details of steering (steer ()), dete rmining the next path 
element via instruction buffer management (ReadNextLine(), GetLine() ), and current 
path updating. After computations are complete, commanded velocities are returned to 
the motion interrupt handling routine and are applied to the robot. 

The new motion mode should behave in a similar manner. The user should be 
able to call a function trackXXX(), which initializes the system for the new mode (XXX). 
This function should be contained in a file like <XXXcmd.c>. After initialization, the 
motion interrupt handling routine would call motionRules(), as before. This function 
should then choose an appropriate motion rule, trackXXXRule(), based on the current 
motion type, TRACKXXXMODE. The trackXXXRule() function should handle the 
specific steering computations (steerXXX()), update the current path, and return 
commanded velocities. Note that since this will be a reactive motion mode, there won't 
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be a next path element, only adjustments to the current path. Figure 15 shows a block 
diagram of this process. 


user.c 


is 


- : : ■■■ H ;f . ; . 

calls trackXXXQ | 



initialize 


Motion system routines 


Motion system data structures 


Interrupt handler function { 
calls motionRules() 

:> 

function motionRules( ) { 
calls trackXXXRule( ) 

Is I I V. $ 


I function trackXXXRule( ) { 
uses steerXXX() 

function steerXXX() { 


Motion interrupt 



Figure 15. New Motion Mode Process. 


The following sections discuss how these template forms (XXX) were 
implemented to support the wall following motion mode. 

1. The User Program Function Call 

At the highest level, the user program initializes the system for wall following by 
calling the function trackWall(sonarNum, offset). This function is used to specify 
whether wall following will occur with the wall on the left side or the right side of the 
robot. It is also used to specify what the offset distance will be from the wall to the robot. 
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In the wall following mode, the robot uses linear fitted data from the wall as its 
navigation source. The trackWall() function is used to set up the sonar system to 
perform this task. The user is able to specify which sonar to use by passing the sonar 
number in through the first argument, sonarNum. This has the added effect of specifying 
which side the wall will be on (#7 for right side, #5 for left side). 

Constants are used in the code instead of sonar numbers to improve readability. 
These constants are defined in the file <sonar.h> and have the form S ###, where the S 
signifies "sonar" and ### is a three digit number specifying the angle of the sonar 
measured clockwise relative to the front of the robot. For example, the right hand sonar 
(#7) is defined as S090, the left hand sonar (#5) is S270, and the front sonar (#0) is S000. 

The second argument to trackWall() is offset and is used to specify the offset 
distance the robot should use when following the wall. This distance is the magnitude of 
the offset measured from the center of the robot to the current linear fitted data 
representing the wall. 

2. The trackWall() Function 

The trackWall() function is located in the new MML file <wallcmd.c> 

(Appendix B). The function prepares the motion system and the sonar system for wall 
following. 

For the motion system setup, it creates and sets field values in a LINE variable 
called pathElement. LINE is a data structure containing fields for the motion mode type, 
a configuration, steering function gain parameters, circular arc parameters, and other 
motion variables and is defined in <definitions.h>. 
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Depending on the sonar number argument, trackWall() first assigns one of two 
values to the Type field: TRACKRWALLMODE for following a wall to the right of the 
robot, or TRACKLWALLMODE for following a wall to the left of the robot. These values 
are enumeration values of MODE, which is defined in <definitions.h>. The offset value 
is also set at this point depending on the sonar number. If the sonar number is S090 then 
die wall will be on the right side. This implies that the robot will be to the left of the 
directed line representing the wall. Recalling the discussion in Chapter m (Section B.6), 
positive distances are to the left of a directed line and negative distances are to the right. 
When the sonar number is S090, the offset will be positive because the robot should 
maintain the offset distance to the left of the wall. On the other hand, when the sonar 
number is S270 , the offset will be negative because the robot should maintain the offset 
distance to the right of the wall. The user specified offset is also checked to make sine its 
value will not cause the edge of the robot to come too close to the wall. If it will, then a 
default value is assigned. 

trackWallf) next initializes pathElement 's configuration to a default value (all 
zeroes) and its steering gain parameters using the robot's <r value. The initial path will be 
a straight line, so the circle parameters of pathElement won't initially be used by the 
motion system. To be safe, the center point is set to zeroes. The radius, however, is used 
to pass the offset value into the motion system. Once pathElement has been set up, it is 
used as an argument to the function setPathElement(). This function is defined in 
<motion.c> and simply copies its argument into currentPath, a static file scope variable 
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used by many of the motion functions. This simple functionality is important because it 
is the only way for a routine outside of <motfon.O to set currentPath. 

After pathElement and currentPath have been set, trackWall() matches 
Yamabico's initial configuration to the initial path segment by calling 
setRobotConfigImm() with pathElemenfs configuration as the parameter. This function 
is located in <motion.c> and immediately sets the robot's configuration to the specified 
argument. 

Finally, trackWall() sets up the sonar system for wall tracking by enabling the 
required sonars ( S000 for both left and right wall following and either S090 or S270 
depending on the sonar number argument) and enabling linear fitting using a decay factor. 
The functions used to do this, EnableSonar() and EnableLinearFittingDecay(), are 
located in files <sonarcard.c> (Appendix A) and <sonarmath.c> respectively. 
EnableSonar() is a hardware related function. It takes a sonar number as an argument 
and adds that sonar to an internally stored enabled sonar list. 

EnableLinearFittingDecayO sets up the specified sonar to perform the linear fitting with 
decay computations discussed in Chapter ID (Section C.2.b). 

At this point, all initialization is complete, control is returned to the user program, 
and the motion and sonar control systems begin handling movement and sonar events. 

3. The wallHugRule( ) and steerRyWall( ) Functions 

Once motion has started, the motion interrupt handling routine will call the 
motionRules() function to obtain commanded velocities for the next motion cycle. In the 
wall following mode, this function will in turn call the new wallHugRule() function, in 
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<motion.c>, which updates the current path element and calculates the commanded 
velocities to keep the robot on the current path. The wallHugRule() function is 
essentially the heart of the wall following motion mode. It uses both the currently 
implemented steer() function and a new steerByWall() function to calculate commanded 
velocities. The steerByWall() function calculates steering function values based on the 
wall represented by the decay version of linear fitting. Both of these new functions form 
the foundation for solving the problem presented in this thesis and are explained in more 
detail in the following sections. 

C. FOLLOWING A STRAIGHT WALL 

In order to follow a wall using sonar, the first thing that must be accomplished is 
finding a way to represent the wall based on the sonar returns. Once this representation is 
found, the next step is to find a way to direct the robot's motion so that it maintain a 
defined offset distance from the wall. 

1. Method for Calculating Linear Representation of Sonar Returns 

Once a sonar has been enabled to perform linear fitting calculations, it calculates 
and maintains the linear data in a structure called SEGMENT_RES_DEC.AY which is 
defined in <sonar.h>. This structure keeps all the important data for the linear fit ting 
with decay calculations such as the number of points in the data set, the moments of the 
set, the decay factor, and the parameters r and a that define the linear fitted line. It also 
has two helper fields, one containing the sonar number and the other a flag indicating 
whether the linear data is usable or not. 
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There is one other important sonar data structure called SONARD, also defined in 
<sonar.h>. It contains all necessary sonar data for a particular sonar including boolean 
fields indicating whether normal and/or decay linear fitting is turned on, and if the sonar 
data has been updated recently. Other fields hold data related to the current sonar return 
including the global position, current and previous range, and the robot's configuration 
(position and orientation) at the time of the reading. There are also fields containing the 
sonar’s configuration relative to the center of the robot. The sonar system maintains an 
external table of these SONARD data structures, one for each sonar and indexed by the 
sonar number, in an array called sonar table[]. This array can be accessed from any file 
that includes the <sonar.h> header file. 

When a sonar interrupt occurs, the sonar interrupt handling routine, 

SonarSysControl() in <sonar.c> (Appendix A), is invoked. This function updates the 
sonar table[] array for the sonars in the currently active sonar group. Of particular 
interest here is that it sets the update field to 1 (updated) and calls the 
LinearFittingDecayO function if the decay linear fitting flag is 1 (enabled). The update 
field is used to alleviate problems associated with the timing differences between the 
motion system and the sonar system interrupts. The LinearFittingDecay() function is 
used to update the linear fitted line representing the sonar data (i.e. the wall). This 
function is located in <sonarmath.c>. It does not actually do the linear calculations but 
is used to perform error checking before calling the AddToSegmentDecay() function, 
which does the work. 
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AddToSegmentDecay() is also located in <sonarmath.c> and, when called, adds 
the new sonar return to the linear data and recomputes die linear fitted line. The 
arguments to this function are the sonar number and the global position of the new sonar 
return. The sonar number is used as an index into the array seg_data_decay[ J which is a 
static file scope array holding the SEGMENT RES DECAY structures for each sonar. 

The function's second argument is the point that will be added to the linear data. If the 
addition of the new point will create a data set with two or more points, 
AddToSegmentDecay() adds the new point to the existing linear moments using the 
procedure described by Equations (3.42 — 3.47), recalculates the centroid and secondary 
moments using Equations (3.26 - 3.29), recomputes the line representation parameters 
using Equations (3.34) and (3.35), and sets the usable field to 1 (true). If the new point is 
the first point in the data set, the function only calculates the linear moments and the 
usable field will remain 0 (false). 

Additional functions are provided in <sonarmath.c> to access or set various 
components of the arrays of structures associated with the linear fitting with decay 
algorithm. Each of these functions takes the sonar number as one argument since they 
reference arrays that are indexed by the sonar number. Table 4 contains a list of these 
functions, the array/structure and field they access, and a short description. 

2. Using Linear Fitted Data to Represent Wall 

The sonar system interrupt handler ensures linear data is kept up to date for those 
sonars that have been enabled for linear fitting. (NOTE: From this point forward, any 
reference to linear fitting, linear data, etc. will mean data associated with the decay 
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version of the linear fitting algorithm.) Any function may obtain a current copy of the 
data by calling GetSegmentDecay(sonarNum). This function returns a copy of the current 
SEGMENT_RES_DECAY structure associated with the specified sonar. Before using the 
data in this structure, it should be checked for usability by testing the usable flag. If 
usable, the r and a values specify the line representing the data points. These values 
specify the distance and normal direction to the line from the global origin. The line 
representation specifies an undirected line. In order to represent a wall, the line must be 
converted to a directed line, such as a line configuration. 


FUNCTION 

Array 

Structure 

Field 

Purpose 

EnableLinearFittingDecay( ) 

sonar table[ ] 
SONARD 

decayFitting 

Sets 

decayFitting 
to 1 

(enabled) 

DisableLinearFittingDecay( ) 

sonar table[ ] 
SONARD 

decayFitting 

Sets 

decayFitting 
to 0 

(disabled) 

GetSeginentDecay ( ) 

seg data decay[ ] 
SEGMENT_RES_DECAY 

Entire 

structure 

Returns a 
copy of the 
structure 

SetDecayFactor( ) 

seg data decay[ ] 
SEGMENT_RES_DECAY 

decayFactor 

Sets 

decayFactor 
to a 

specified 

value 

ResetMomentsDecay( ) 

seg data decay[ 3 
SEGMENT_RES_DECAY 

Entire 

structure 

Resets all 
fields to 
default 
values and 
usable to 0 
(false) 


Table 4. Access and Set Functions of Linear Fitting w/ Decay 
Algorithm. 


To convert the undirected line to a line configuration representing the wall, values 


for the x and y coordinates, orientation ((3), and curvature (k) must be found. Since it is a 
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line, its curvature will be zero. The x and y coordinates are easily converted from r and a 
through the relations: 

x = rcosa (4.1) 

y = rsina (4.2) 

The wall's orientation (p) depends on the robot’s orientation (0). The two possible 
values for P are (a + n/2) and (a - rc/2). Figure 16 shows examples of both situations. 



To determine the proper choice, one of these values is compared to 0. Touring 
wall following, the robot's orientation will be close to the wall's orientation and definitely 
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less than 90°. Therefore, the correct choice is found by comparing the magnitude of the 
difference between the two possible values and the robot's orientation: 


n 


if 


Oil a 


n 


-9 


7C 


Jt 


J3= a+ — , otherwise 
2 


(4.3) 


where 0() is the normalization function for angles. 

The wall configuration has now been extracted from the sonar data and can be 
defined as (x, y, p, 0). 

3. Following the Wall 

In order to follow a wall using sonar, the procedure described in the last section is 
implemented each time new sonar data is available. In the wall following motion mode, 
the wallHugRule() function is called during every motion control cycle. This function is 
responsible for maintaining the current path element and calculating the commanded 
velocities that will keep the robot on this path. 

Since the motion interrupt handler runs approximately five times between sonar 
interrupts when one sonar group is active, calculating a new configuration for the wall 
will only occur when new sonar data is available. Each time wallHugRule() is called, it 
first checks if the sonar data has been updated. If not, the current wall configuration 
remains unchanged. If new sonar data has been received, then the function calculates the 
new wall configuration using the procedure described in the last section. 
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The current path is calculated using the wall configuration and the specified offset 
distance. As stated in Section B.2, the offset value is passed into the motion system by 
the initialization function trackWall(), through the radius field of the path element. The 
first time wallHugRule() is invoked, the offset value is stored in the function scope static 
variable offset. The current path is computed by: 

current path = wall config ° (0, offset, 0,0) (4.4) 

This places the current path parallel to the wall and on the appropriate side at the 
correct distance since offset is negative for left wall following and positive for right wall 
following. 

The commanded velocities are found by using the function steerByWall(). This 
function is called with the robot's configuration, the wall configuration, and the offset. It 
returns a steering function value which is used to calculate the commanded velocities. It 
is identical in functionality to the normal steer() function for a line. It uses the distance 
to the wall minus the offset distance as the distance feedback and the difference between 
the robot's orientation and the wall's orientation as the angular feedback. 

The process described in this section occurs during each motion control cycle and 
produces the desired effect of following a wall with a sonar. The "straightness" of the 
wall can vary slightly without loss of wall following ability because the linear fitting with 
decay algorithm makes timely adjustments to the linear representation of the wall and the 
robot adjusts its path accordingly. 
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D. ORTHOGONAL WALLS AND OBSTACLE AVOIDANCE 


Once the straight wall following algorithm was created, it was used to expand the 
solution scope to include following walls with orthogonal turns. To solve this problem 
the robot uses the wall following algorithm while following straight wall segments until a 
turn is sensed. Turns are either concave (inside) or convex (outside). When a turn is 
required, the robot stops executing the wall following algorithm and enters a multi-phase 
process using lines, circular arcs, and the normal steer() function to navigate inside or 
around the comer of the wall. When the robot is abreast of a new wall, it returns to the 
wall following mode. 

The sections that follow explain how turns are sensed, how the turn is 
accomplished, and how the determination is made to return to wall following. 

1. Concave Corners 

Concave comers are sensed by Yamabico's forward looking sonar (S000). The 
criteria used to start an inside turn comes from the neutral switching point idea discussed 
in Chapter IQ (Section B.8). When the robot is following a wall and will make an inside 
turn to the next wall, the next path will 90° to the left or right of the current path. Figure 
17 depicts this situation when the robot is in the left wall following mode. 

The next path will also be offset from the next wall toward the robot's current 
position. In normal path tracking mode, the neutral switching point occurs when the 
steering function for the current line equals the steering function for the next line. 

Because the difference in angles between the current and next segment is known, an 
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Figure 17. Neutral Switching Point from Current Path to 
Next Path. 

expression can be found for the optimal distance to start a turn. This is accomplished by 
equating the two steering functions and solving for distance. The expression yielded by 
this was solved by Dr. Kanayama [2]: 

d= 3aa (4.5) 

where d is the optimal turn distance (signed), a is the angular difference (rc/2), and 
a is the sigma value used in the turn. 

The forward sonar is used to sense when this distance occurs. However, since the 
next path will be offset from the wall and the sonar senses the wall, not the path, the 
distance at which the robot should start its turn is: 
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(4.6) 


d = 3^a+\offset\ 

When the forward sonar senses a distance less than or equal to this value, the 
robot stops following the current wall (turns the side sonar off), computes an approximate 
next path line (L), sets the current path to L, and tracks the new current path using the 
normal steer() function. This next line is computed relative to the robot's configuration 
by: 

L = robot config ° (d/s/000 - \offset\,0, AOfi) (4.7) 

where distOOO is the forward sonar distance to the wall and A0 is ±k/2 depending 
on which side the wall is on. 

While in the turn toward the new current path, the robot orientation is compared 
to that of the current path. Once this difference is less than a specified value (~10°), the 
robot is in a transition area. The side sonar is turned on and linear data for that sonar is 
reset via ResetMomentsDecay(). While in this transition area, the robot continues to 
track the current path until sufficient linear data has been built for the next wall. 

When there are enough data points in the linear fitting algorithm, the robot returns 
to the wall following mode. 

Figure 18 shows the phases of a concave turn. To implement these phases, 
additional code was incorporated in the function wallHugRule(). Static flag variables 
were added to perform checks on turn phase status (in turn, in transition) and turn type 
(inside, outside). A non-static flag was also added to determine the track type (following 
wall, current path). This flag is set each time the function is called and is used to 
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distinguish which type of calculations to use, the wall following algorithm or the normal 
path tracking algorithm. 



offset; 1 


•3 


offset 


1. Robot following wall 

2. Turn toward calculated line (L) 

- front sonar range (d) = optimal turn range 

- side sonar off 

3. In transition area (within ~10 degrees) 

- side sonar on 

- start building new linear data 

- keep tracking line L 

4. Robot following new wall (good linear data) 


Figure 18. Phases of a Concave (Inside) Turn. 


2. Convex Corners 

Convex comers are sensed by the side sonar. As the robot proceeds along a 
straight wall, sonar distance readings vary only slightly. When the robot moves past a 
convex comer, the sonar distance measurement will jump from a fairly stable reading to a 
much larger distance. A convex turn is started when the difference between the current 
and previous sonar readings exceeds a certain threshold value. 

The sequence of turn phases that occur during a convex turn are similar to those 
of the concave turn, but with an extra phase. When the threshold value is exceeded, the 
side sonar is turned off, the current path is changed to a circular arc, and the robot tracks 
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the current path around the comer. The circular arc is defined not only with a 
configuration, but also with a center position and radius. The arc's configuration is 
defined using the robot's current position and orientation with the curvature equal to the 
negative reciprocal of the offset value: 


current path config = 


robot x, robot y, robot 0,- 


offsetj 


(4.8) 


This gives the circular arc the proper direction (clockwise, counter-clockwise) 
with a radius equivalent to the offset distance. The current path structure also maintain a 
radius field and a center position field. These fields were unimportant in the wall 
following and concave turn implementations because only line paths were used, but must 
be set in this situation. The radius value is simply the reciprocal of the curvature. The 
center position is found by extracting the position from the following composition: 


current path config ° (0-offset,0,0) (4.9) 

The robot tracks the current path (arc) for approximately 90° which should 
theoretically put it abreast the wall comer heading in the proper direction for the next 
wall. To allow for motion errors, however, the current path is set to a line configuration 
starting at the robot's current position with the robot's orientation. This causes the robot 
to track straight ahead from its current position. 

The robot tracks the current path (line) for a specified distance to avoid the two 
situations depicted in Figure 19. 
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Figure 19. Undesirable Possibilities of a Convex (Outside) 
Turn. 


To determine when the specified distance has been traversed, the relative 
configuration of the robot to the current path is computed by: 

relative config = [current path config)' 1 o robot config (4.10) 

When the x coordinate of this relative configuration is greater than the specified 
distance, the robot enters a transition area similar to the one specified in the concave turn. 
The side sonar is turned on and the linear data for that sonar is reset. While new linear 
data is being built, the robot continues to track the current path. 

After sufficient linear data has been accumulated, the robot returns to the wall 
following mode and tracks the new wall. 

Figure 20 shows the phases of a convex turn. To implement these phases, code 
was again added to the function wallHugRule(). The static flag variables continued to be 
used to perform turn phase and turn type checks. An extra value was included for the 
turn type flag to represent when the robot is tracking the circular arc. The track type flag 
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1. Robot following wall 

2. Track circular arc (radius = offset) 

- side sonar distance difference |d - d0| > threshold 

- side sonar off 
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- side sonar on 

- start building new linear data 

- keep tracking line L 

5. Robot following new wall (good linear data) 


Figure 20. Phases of a Convex (Outside) Turn. 


was not changed. Table 5 shows the values of the flags for the different phases in 
concave and convex turns. 

3. Obstacle Avoidance 

In the wall following mode, any obstacle in the robot's path is inherently close to 
the wall. By implementing the capability to navigate by following a wall and around 
orthogonal comers, obstacle avoidance is automatically included. The obstacle can be 
incorporated as part of the wall (Figure 21) and the robot uses the new capabilities to 
steer around it. 
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Table 5. Values of Flag Variables in wallHugRule( ) During 
Phases of Turns. 



Figure 21. Examples of Obstacles in Wall Following Path. 
4. Analysis of the wallHugRule() Function 


As previously stated, the wallHugRule() function, located in <motion.c>, is the 
heart of the wall following motion mode. It is called by the motionRules() function on 
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every motion cycle when the wall following mode is in use and returns the commanded 
velocities that keep the robot on the correct path. The function handles all the phases of 
wall following (straight wall following, concave turns, convex turns) discussed in this 
chapter. Because this function is so important, a detailed analysis of the code is 
warranted. The code for this function is listed in Appendix C with reference line 
numbers. In die analysis that follows, line numbers will be indicated by specific 
reference (e.g. line ##) or by a number in parentheses. 

Line 9 is the beginning of the function header. It specifies that the function will 
return the commanded velocities (linear and rotational) in a structure called VELOCITY. 
The function accepts as arguments (line 10) the actual and commanded velocities used by 
the robot during the previous motion control cycle. 

Lines 13-48 declare the variables used in the function. The CONFIGURATION 
variables are used to define and manipulate wall representation, offset, relative 
positioning, inverse, and temporary configurations during code execution. Linear fitted 
data from the sonar is extracted into the curSeg variable. Wall configuration components 

are stored in the static wall _variables defined in line 16 and the previous angle of the 

wall is kept in the static variable prevWallTheta (line 17). Line 19 defines a static 
variable to store the initial orientation value of a circular arc. The deltaDistance variable 
is used in the final computation of the commanded velocities and deltaAngle is used to 
determine which way the robot will turn in various situations depending on which side 
the wall is on. While in a turn, the robot's sigma value differs from the value used for 
straight wall following. savedSigma saves the old value while the robot turns. 
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pingedOnce and pingedTwice are flags used to ensure good history data from the sonar. 
firstTimeCalled is only true the first time the function is called. The next two lines (28, 
29) declare variables that hold the side sonar number and sonar distances, offset stores 
the required offset distance the robot will maintain from the wall. This value is negative 
for left wall following and positive for right wall following. Lines 37-43 define the flag 
variables which are used to specify which motion state the vehicle is in. The first of 
these, halted, is used in collision detection. The next four are the turn phase indicators 
depicted in Table 5. The final three function variables are used in steering calculations. 
jL holds the steering function value, kk and kk2 hold the current path's kappa and kappa 
squared values, and k is the steering feedback gain parameter. 

After the variable declarations, the function first tests which type of wall 
following (left or right) is in use through the currentPath structure. This structure is a 
static file scope variable which stores the current path information during each motion 
control cycle. The switch statement in lines 53-66 assigns the appropriate values to 
sonarNum and deltaAngle depending on the wall following mode. 

It was stated earlier that the offset value is passed into the motion control system 
through the radius field of currentPath. This takes place in lines 69-71. When the 
function is first called, the offset value is copied from currentPath.Rad.ius into offset. 

Line 74 extracts the linear fitted data from the sonar system and places it in 
curSeg using the <sonarmath.O function GetSegmentDecay(). Once this data available, 
lines 77-82 calculate the orientation of the wall using Equation (4.3). 
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Before any motion calculations take place, lines 86-93 check for a possible 
upcoming collision. If the forward sonar has been updated and the distance it measures is 
less than a predefined threshold, the halted flag is set, commanded velocities are set to 
zero, and the function returns. 

To ensure good sonar data history, lines 96-104 determine if the side sonar has 
been updated (pinged) zero, one, or two times and sets the flags pingedOnce and 
pingedTwice appropriately. 

The next section of code (112-247) performs the motion phase determination and 
adjusts the currentPath to keep the robot on course. The outer if statement breaks down 
the phases into blocks for wall following (112-180), in turn (183-230), and in transition 
area (233-247). 

If the inTurn flag is not set, the robot is in the wall following mode, and the first 
code block is entered. The code in this block first sets the sonar distance variables. The 
front sonar distance is compensated for displacement from the robot center (115). The 
current and previous sonar readings are taken in raw form. These values are used in the 
nested if statement to check for turn indications. The if statement on lines 121,122 first 
checks that the sonar has been updated and then uses the criteria described by Equation 
(4.6) to determine if the robot should begin an inside (concave) turn. When the criteria is 
met, the inTurn flag is set, the turnType is set to the value for an inside turn, and the 
current sigma value is saved and then set to a predefined value. Lines 127-131 update the 
currentPath configuration using the model described in Equation (4.7). Because the 
sigma value was changed, lines 132-135 update the steering parameters of currentPath to 
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reflect the change. Finally, because the robot is now in a turn, the trackType flag is set to 
1 to signify that the normal steer () function should be used to track the current path. The 
next two lines finish the code block by resetting the sonar update field and disabling the 
side sonar. 

If the criteria for an inside turn is not met, the code next checks for an outside 
turn. Lines 144-146 check if the sonar has been updated and has good history data, and if 
the difference between the current and previous returns exceeds a predefined threshold. If 
all these conditions are met, the robot will enter an outside turn. Lines 147-150 set the 
inTurn flag, save and reset the robot sigma, and set the turnType variable to the value 2 
indicating the robot is on a circular path. At this point, currentPath is changed to a 
circular arc (155-163). The initial angle is saved for later comparison, the kappa value is 
calculated, and the steering feedback gains are set by lines 155-161. Note here that the 

3 

value used for the radius is actually — offset instead of offset. This was a late change to 

the code but allows the robot to make a tighter turn around the comer. Lines 162,163 set 
the configuration of currentPath as described by Equation (4.8). The center and radius of 
currentPath are set in lines 165-169 as prescribed by Equation (4.9) and its preceding 
paragraph. The next three lines (171-173) set the trackType to 1 (track current path), 
reset the sonar update field, and disable the side sonar. 

If neither criteria (inside or outside turn) is met, the robot simply continues to 
follow the wall (lines 177, 178). 
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The brace on line 180 closes the first block of code. The second block of code 
begins on line 183 and handles the phase of motion when the robot is in a turn but not yet 
in the transition area. 

If the robot is in an inside turn ( turnType — 0) and the robot's orientation is 
within a predefined threshold of the current path (lines 188-190), then the inTransition 
flag is set, the side sonar is turned on, and the linear fitting data and pinged variables 
are reset by lines 191-194. 

If not on an inside turn, line 197 checks if the robot is on an outside turn (phase 3 
in Figure 20). If so, then the procedure described by Equation (4.10) and its following 
paragraph is performed by lines 198-201. If the check in line 201 is passed, then the 
robot is in transition and lines 202-205 set the inTransition flag, turn on the sonar, and 
reset the linear data and pinged _variables. 

When the robot is on the circular arc of an outside turn ( turnType — 2), the first 
condition of the if statement on lines 211-215 is met. The next two conditions check if 
the robot's orientation has changed by ( deltaAngle ±3°) from either the circular arc's 
starting angle or the previous wall's angle. The angular error factor was empirically 
selected through testing. If either of these criteria is met, the robot proceeds straight 
ahead on the straight portion of an outside turn ( turnType = 1). Lines 217-224 update 
currentPath and turnType accordingly. 

For all three situations in which the robot is in a turn, the trackType flag is set to 1 
(line 229). 
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The brace on line 230 closes the second block of code. The third and final block 
handles the motion phase when the robot is in the transition area of a turn and begins on 
line 233. 

While in the transition phase, the linear fitting data is being built. When the data 
becomes usable, the sonar system will set the usable flag. Line 236 checks this flag, and 
if the data is usable, the robot will return to the wall following mode using the old sigma 
value (lines 237-240). If the data is not yet usable, the robot remains in the transition 
phase (line 245). 

The brace on line 247 closes the third motion phase block of code. All that is left 
to do is to calculate the commanded velocities that will be returned. 

The switch statement on lines 249-278 calculate the steering function value using 
either the steerByWall() or steer() function. The correct steering function is chosen by 
the status of the trackType variable. For the wall following mode {trackType = 0), the 
wall configuration is first defined. Lines 252,253 calculate the wall's x and y coordinates 
using Equations (4.1) and (4.2). The wall's angle was already computed so line 255 uses 
these values to define curWall. Lines 259,260 update currentPath using Equation (4.4) 
and line 263 retrieves the steering function value using steerByWall(). Finally, the wall's 
current orientation is saved in prevWallTheta (line 266) to be available for future use. 

If the trackType is 1, the current path will have already computed and the steering 
function value is found using the normal steer () function (line 271), allowing the robot to 
track the current path. 
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After the default case for the switch statement (just a precaution) come the final 
lines in the function. Line 280 always sets firstTimeCalled to zero since the function is 
only called for the first time once. Line 282 uses a motion system function to calculate 
the linear portion of the commanded velocities and line 284 calculates the distance the 
robot has traveled over the last motion control cycle. This distance is combined with the 
steering function value, the robot's current curvature, and the commanded linear velocity 
to compute the required rotational velocity (lines 286,287). Once the commanded 
velocities have been determined, the wallHugRule() function is complete and the 
commanded velocities are returned to the calling routine. 

E. USER PROGRAM 

The previous sections described how the wall following mode is implemented and 
used to set robot on course. The user program, included in Appendix B, is used for 
operator interaction for setting up initial system parameters and for end of run condition 
checks. Before calling the trackWall() function, the user program prompts the operator 
for system parameters such as whether sonar logging is on, the desired wall offset, the 
sigma value, the linear decay factor, and which side the wall will be on. The program 
uses this information to initialize corresponding parameters in the software system. The 
trackWall() is then used to initialize the wall following mode. At this point the robot 
begins its motion, and the remaining portion of the user program monitors the robot's 
progress until end of run conditions are met. Once the robot has either halted or 
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completed a complete circuit, the program shuts down the sonar system, stops the motion 
processing, and returns to the main program to download logged data. 
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V. THESIS RESULTS AND CONCLUSIONS 


A. RESULTS 

The goal of this thesis was to give an autonomous vehicle, such as Yamabico, the 
capability to navigate about a room or rooms composed of continuously connected 
orthogonal wall segments while avoiding obstacles in its path. The result of the research 
effort in implementing this behavior produced the desired effect By using the new 
motion mode now incorporated in the MML software library, Yamabico can navigate 
around such a room in a safe, smooth, and efficient manner. Several tests were conducted 
to verify this fact and the data collected supports this conclusion. 

Figure 22 shows actual data collected from Yamabico while traversing a hallway. 
The x and y axes are measured in cm. Both runs were completed using the left wall 
following mode. The first run used an offset of 80 cm while the run in the second run 
used a 50 cm offset. Both runs demonstrate the ability to maintain an offset distance 
while traversing a straight wall and smoothly negotiate both minor variations in the wall 
and concave comers. The angular tilt in the first figure is caused by initial positioning of 
the robot and errors associated with dead reckoning (DR) navigation. Even though these 
errors would be significant in normal path tracking mode, they are minor in the wall 
following mode since the robot is maintaining the appropriate distance from the reference 
wall, as indicated by the wall data being tilted by an equal amount. 
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Figure 22. Left Wall Following with 80cm and 50cm Offsets. 


80 


















To demonstrate the ability to negotiate a convex turn as well as obstacle 
avoidance, Figure 23 shows the motion and sonar data of a run starting in the comer of a 
room with convex comers, extending out and around an outside hallway con taining an 
obstacle, and then back into the room completing the circuit. This run was performed 
using right wall following with an offset distance of 50 cm. The robot once a gain 
maintains the correct offset distance from straight wall segments while smoo thl y 
negotiating both concave and convex comers. As before, DR navigation errors 
accumulate during this extended run, indicated by the increasing tilt of the walls as the 
run progresses and the difference between the starting and fini s hing positions. The robot, 
however, maintains the correct offset distance from the reference walls throughout the 
run. The obstacle is treated as just another series of turns. 
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Yamabico Wall Following Data (6/01/99) 
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Figure 23. Complete Circuit of Two Rooms Using Right Wall 
Following with 50cm Offset. 
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With the successful implementation of a wall following motion mode, a solid 
foundation has been formed for the development of many advanced abilities. 

B. FUTURE WORK 

The work completed in this thesis was accomplished with fairly broad goals in 
mind and, although important, should be considered as a first step toward more complex 
behaviors. Some ideas for future work are presented in the following sections. 

1. Starting Point 

During the development of the wall following mode, the robot's starting point was 
always in close proximity to the first wall. For a more versatile behavior, the starting 
point should not have this limitation. An algorithm could be developed allowing the 
robot to start anywhere inside a room and search for the first wall. One possible 
implementation might be to have the robot proceed straight ahead until encountering a 
wall and then make a decision whether to turn right or left to enter the wall following 
mode. 

2. Incorporate an S-turn Ability 

While the robot is following a wall, minor variations in the wall cause the robot to 
adjust its course to maintain the offset distance. If the robot senses a large difference in 
side sonar distance between the current and previous readings, it enters a convex turn. 
There is a range of distances that is less than the threshold for convex turns but too large 
for the robot to compensate for. A possible fix to this problem is to incorporate an S-tum 
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when this situation arises. The robot could turn 45° toward the wall and then reverse this 
turn to again match the angle of the wall. 

3. Other Than Orthogonal Turns 

An obvious extension to the work done in this thesis would be to develop a more 
complex algorithm that would handle a wider variety of turn angles. During the 
development of the straight wall following algorithm, testing was performed on walls that 
varied in angle by values up to 20°. The robot compensated fairly well for angle 
differences of 15° degrees and below. Modifying the orthogonal turn algorithms to 
handle turns of 30° should result in a very robust behavior. 

4. Better Collision Detection 

The current collision detection mechanism is rather elementary. The robot uses its 
forward looking sonar to detect a situation where, by some error or miscalculation, it is 
too close to something directly ahead. This collision detection mechanism could be 
improved by use of additional sonars, such as those angled ±30° of the nose. 

5. Very Small Obstacle Avoidance 

The robot is capable of avoiding obstacles in its path as long as those obstacles are 
seen by the sensors. Because the forward looking sonar is positioned on the centerline of 
the robot, small obstacles that are positioned inside the sonar, but in the path of the wheel 
closest to the wall, will be hidden from the sensor and will cause a collision. The solution 
to this problem is not simple. One idea would be to use one of the quartering sonars as a 
warning detector and have the robot make adjustments to avoid a collision. Another idea 
would be to add forward looking sonars at the comers of the robot. This would very 
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difficult since it would require a hardware change and modification of the software- 
hardware interface. 
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APPENDIX A: MML FILES - MODIFIED OR IMPLEMENTATION RELATED 


/*****★********************•**********************************************i 


/* File: constants.h */ 

/* */ 

/* last update */ 

/* June 3, 1993 by Dave MacPherson */ 

/* Wed 04-27-94 Frank Kelbe - update for mmlll */ 

/* Sun 09-18-94 Cleaned up for clean irtmlll version */ 

/* May, 1999 Dan Wells - added constants for linear fit w/ decay */ 

/* */ 


/************************************************************************ j 

#ifndef CONSTANTS_H 

tdefine _CONSTANTS_H 

/* default motion logging filename */ 
fdefine MOTIONLOGFILE "motion.log" 

/* there is no default sonar logging filename, because the type of logging 
must be specified for sonar logging, and the default filename is based 
on the type of logging to be performed */ 


/* default motion logging buffer size (128K) */ 
tdefine MOTIONLOGSIZE 0x20000 

/* Sonar logging buffer size (64K) */ 

#define SONARLOGSIZE 0x10000 

/* the default 10 buffer size (64K) */ 

#define TRACELOGSIZE 0x10000 


/*** system limits ***/ 

/* max number of sequential instructions that can go in the buffer */ 

#define INST_MAX 500 

/* raax number of tracing/logging buffers in the system at once */ 

#define MAXIOBUFFERS 20 

#define DEFAtJLT_PRECI SI ON 6 

tdefine MAX_REAL_PRECISION 15 

#define MAX_INTEGER_DIGITS 19 

/* defines the length of time (in seconds) of the motion control cycle */ 
#define MOTION_CONTROL CYCLE 0.01 


/* generic defined constants */ 


/* #define NULL 0 */ 

/* old def of NULL above. Standard C def below */ 
#ifndef NULL 

#define NULL ((void *)0) 

#endif 


#define INFINITY 
#define INFINITY0 
#define PI 
#define DPI 


999999 

99999 

3.14159265358979323846 
6.28318530717958647692 /* PI * 2 


*/ 


/******************************************************* j 
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/*************★*******★*********************************y 


/* Just info - not used anywhere - right now */ 


/*** machine limits ** 
#define DBL_MIN 
#define DBL_MAX 
#define INFINITE 
#define INF 

/* Numerical Constants 

#define HPI 
#define PI34 
#define PI4 
#define RAD 
#define DAR 


/ 

2.2250738585072014e-308 
1.797 6931348623157e+308 
DBL_MAX 
DBL_MAX 

*/ 

1.57079632679489661923 

2.35619449019234492885 

0.78539816339744830962 

57.29577951308232087684 

0.017453292 


/* 

PI/2 

*/ 

/* 

3PI/4 

*/ 

/* 

PI/4 

*/ 

/* 

180 / PI 

V 

/* 

PI / 180 

V 


/****★***★★★*★★★*★*********★****★*★★**★*★***★★****★**★******■********** ^ 
/* constants defination for track.c */ 

tdefine SO 0.0 

tdefine r2d 57.29577951 

#define d2r 0.017453292 

#define bufferMax 100 

#define QPI PI/4.0 

/*********************************************************************y 


/* constants for linear fitting with decay and wall following 

V 

/* Added by Dan Wells, May 

99 



*/ 

#define 

DEFAULT DECAY 

0.9f 




#define 

DEFAULT WALL OFFSET 

80 

/* 

from robot center */ 


#define 

MIN EDGE OFFSET 

10 

/* 

from robot edge */ 


#define 

INSIDE TURN SIGMA 

5.Of 

/* 

for tighter inside turns */ 


#define 

OUTSIDE TURN SIGMA 

10.Of 

/* 

for outside turns */ 


#define 

COLLISION THRESH 

30 

/* 

distance from front sonar */ 


tdefine 

TRANSITION ANGLE 

0.17453f 

/* 10 degrees */ 


#define 

OUTSIDE TURN THRESH 

80 

/* 

diff betwn cur and prev sonar 

retrn */ 

#endif 







/**★*****★★****★**★***★****★****************★********************************** 
Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: December 8, 1993 

Revised: May 3, 1994 by Kevin Huggins 

May, 1999 by Dan Wells - wall following motion mode 
File Name: definitions.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 
Description: This file contains standard definitions and data type 

declarations used throughout the reset of the MML system. 

******************************************************************************i 


#ifndef _DEFINITIONS_H 

#define _DEFINITIONS_H 

/* Always include this because it is needed by most modules */ 
#include "constants.h" 

typedef unsigned char BYTE; 
typedef unsigned short WORD; 
typedef unsigned long LONG; 

typedef enum { NOMODE, 

ENDMODE, 

STOPMODE, 
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TRACKMODE, 

TRACKSMODE, 

TRACKRWALLMODE, 

TRACKLWALLMODE, 

ROTATEMODE, 

PARAMODE, 

BIDIRMODE, 

KS PIRALMODE, 

FOLLOWMODE, 

REGIONMODE, 

PCMODE 
} MODE; 

typedef enum { NOCLASS, 

LINECLASS, 

CIRCLECLASS, 

BLINECLASS, 

NOSTOPBLINECLASS, 

BIDIRCLASS, 

LEFT, 

RIGHT, 

CENTER, 

CCWLEFT, 

CCWRIGHT, 

CWLEFT, 

CWRIGHT 

} CLASS; 

typedef enum { INITMOTION, 

INTERMOTION, 
INTRAMOTION, 
FINALMOTION 
} STAGE; 


typedef enum {FALSE = 0, TRUE} BOOLEAN 


typedef struct { 
MODE mode; 

CLASS class; 

} PATH TYPE; 


typedef struct { 
double Linear; 
double Rotational; 
} VELOCITY; 


typedef struct { 
double X; 
double Y; 

} POINT; 


typedef struct{ 
POINT Posit; 
double Theta; 
double Kappa; 
} CONFIGURATION; 


typedef struct{ 

POINT Focus; 

CONFIGURATION Directrix; 
} PARA; 

typedef struct{ 

CONFIGURATION config; 



PATHJTYPE 
} PATH_ELEMENT? 


pathType; 


typedef struct{ 

POINT LeftStart; 

POINT LeftEnd; 
double LeftOrient; 

POINT RightStart? 

POINT RightEnd; 
double RightOrient; 

} REGIONEDGE; 

typedef struct Image{ 

POINT coord; 

int ImageType; 

double Orient; 

double ClosedDist; 

} Image; 

/* Error Codes */ 

typedef enum { 

NOERROR, 

ECODEO, /* configurations too close */ 

EC0DE1, /* erroneous goal configuration orientation */ 
ECODE2, /* SSTOP function detected in moving state */ 
ECODE3 /* undefined instruction class */ 

} ERROR_CODE; 

/*ADDEDV 

typedef struct { 

MODE 

CONFIGURATION 
double 
double 
double 
double 
POINT 
double 
int 

} LINE; 

#endif 


Author(s): Kevin Huggins 

Project: Yamabico Robot Control System 

Date: April 14, 1994 

Revised: May 3, 1994 

File Name: geometry.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 
Description: This file contains the standard geometry functions that are 
called by several functions. 

******************************************************************************/ 

#include "definitions.h" 
tinclude "math.h" 

#include "geometry.h" 
linclude "motion.h" 

#include "stdiosys.h" 
tinclude "constants.h" 

/*★★★*★★★★★★★★*★★*★★***■*•★★★*★★♦★*★*★★***★★***★**•*■**★**★★★★★★**★★***★★*★***★**★* 
Function: eu_dis() 

Purpose: Computes the Eucledian distance between two given points 
Parameters: double xl,yl,x2,y2 
Returns: double 


Type; 

config; 

a; 

b; 

c; 

Radius? 

Center? 

speed; 

direction; 
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Comments: 

******************************************************************************j 

double 

eu_dis(double xl, double yl,double x2, double y2) 

return sqrt(((xl - x2) * (xl - x2)) + ((yl - y 2) * (yl - y2})); 


/****★*************-************************************************************ 
FUNCTION: norm() 

PARAMETERS: double angle - the angle to normalize 

PURPOSE: normalize the input angle between -PI and PI 

RETURNS: double: the normalized angle in radians 

COMMENTS: This is the most common normalizing function used in the system 

This performs that same as norm() and normalize)() in MML10. 

******************************************************************************f 

double 

norm(double angle) 

/*{ 

while ((angle > PI) || (angle <= -PI)) 

{ 

if (angle > PI) 
angle -= DPI; 
else 

angle += DPI; 

} 

return angle; 

}*/ 


return(angle - DPI*(ceil((angle + PI)/DPI) - 1.0)); 

> 


/*********★**********************★*********★*********************************** 
FUNCTION: positiveNorm() 

PARAMETERS: double angle - the angle to normalize 

PURPOSE: normalize the input angle between 0 and 2PI 

RETURNS: double: the normalized angle in radians 

COMMENTS: Same functionality as pnorm() in MML10 

*************************************** ***************************************^ 
double 

positiveNorm(double angle) 

{ 

while ((angle >= DPI) || (angle < 0)) 

{ 

if (angle >= DPI) 
angle -- DPI; 
else 

angle += DPI; 

} 

return angle; 


/****************************************************************************** 
FUNCTION: negativeNorm() 

PARAMETERS: double angle - the angle to normalize 

PURPOSE: normalize the input angle between -2PI and 0 

RETURNS: double: the normalized angle in radians 

COMMENTS: Same functionality as nnormO in MML10 

****************************************************************************** j 

double 

negativeNorm(double angle) 

{ 

while ((angle > 0) || (angle <= -DPI)) 

{ 

if (angle > 0) 
angle -= DPI; 
else 
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angle +- DPI; 

} 

return angle; 

} 

/****************************************************************★************* 
FUNCTION: normPIover2() 

PARAMETERS: double angle -the angle to normalize 

PURPOSE: normalize the input angle between -PI/2 and PI/2 

RETURNS: double: the normalized angle in radians 

COMMENTS: This was designed specifically for parabola calculations 

By Mahmoud Wahdan 3/12/96 

******************************************************************************/ 

double 

normPlover2(double angle) 

{ 

while ((angle > PI/2) || (angle <= -PI/2)) 

{ 

if (angle > PI/2) 
angle -= PI; 
else 

angle += PI; 

} 

return angle; 


/***************************************************************************** 
FUNCTION: signedDist() 

PARAMETERS: CONFIGURATIONS directerix 
POINT focus 

PURPOSE: to calculate the size(signed) distance between a point(focus) and 

a configuration (directrix). 

RETURNS: double: the signed difference 

COMMENTS: This was designed specifically for parabola calculations 

By Mahmoud Wahdan 3/12/96 

*******************************************************************************/ 


double 

signedDist(POINT pt, CONFIGURATION config) 
{ 


} 


return (-(pt.X - config.Posit.X) * sin(config.Theta) + 

(pt.Y - config.Posit.Y)* 


cos(config.Theta)); 


/****************************************************************************** 
FUNCTION: defineConfig() 

PARAMETERS: double x,y,theta,kappa —The values that define a 

configuration 

PURPOSE: To allocate nad assign a configuration 

RETURNS: CONFIGURATION: a configuration 

COMMENTS: Was called def_configuration() in MML10 

********************************************************************★**★★*****/ 
CONFIGURATION 

defineConfig(double x,double y,double theta,double kappa) 

{ 

CONFIGURATION newConfig; 

newConfig.Posit.X = x; 
newConfig.Posit.Y = y; 
newConfig.Theta = theta; 
newConfig.Kappa = kappa; 

return newConfig; 

} 


^****************************************************************************** 


FUNCTION: 
PARAMETERS: 

PURPOSE: 

RETURNS: 


defineParabola() 

double xf,yf -defines the focus 

double xd, yd, thetad -defines the directrix 

To allocate assign a parabola 
PARA: a parabola type 
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COMMENTS: Was called defjparabola() in MML10 

************************************************************,******,**,* i *****^ + . fr ^. 

PARA 

defineParabola(double xf,double yf, double xd, 
double yd, double td) 


PARA newPara; 

newPara.Focus.X = xf; 
newPara.Focus.Y = yf; 
newPara.Directrix.Posit.X - xd; 
newPara.Directrix.Posit.Y = yd; 
newPara.Directrix.Theta = td; 
newPara.Directrix.Kappa = 0.0; 
return newPara; 


Z^*********************************************************^*****^,^^^^^^^ 

FUNCTION: reverseOrientation() 

PARAMETERS: CONFIGURATION original — the original configuration 

orientation changed by 180 degrees 
PURPOSE: To reverse the orientation of a given configuration 

RETURNS: CONFIGURATION: the reversed configuration 

COMMENTS: Was called negate() in MML10 

************ ****************************************************.*****^*. fr * + ^^ j 

CONFIGURATION 

reverseOrientation(CONFIGURATION original) 

CONFIGURATION reversed; 

reversed.Posit.X = original.Posit.X; 
reversed.Posit.Y = original.Posit.Y; 
reversed.Theta = norm(original.Theta + PI); 
reversed.Kappa = original.Kappa; 
return reversed; 


/************************** **************************************************** 
FUNCTION: findSymConfig() 

PARAMETERS: double a — distance from either point to the intersection of 

both lines determined by the two configurations 
double alpha —The angular difference between both orientations 
PURPOSE: This function finds the symmetric configuration of a configuration 

described by alpha and a above. 

RETURNS: CONFIGURATION: symConfig —the symmetic configuration 

COMMENTS: Was called def_sym() in MML10 

One drawback to this function is that it is not possible to 
represent a symmetric configuration whose alpha is equal to PI. 
find_symConfigl () is used to cover these situations 

************ + ***+******ie**-k*i'-k*-k-k-i'*-feic**ie*-ifkic*-/c1e*****i'-kit**-ir1t’k1c*******ie1rie1rie1t***ie/ 

CONFIGURATION 

findSymConfig(double a, double alpha) 

{ 

^ return defineConfig(a * (1.+ cos(alpha)), a * sin(alpha), alpha, 0.0); 

/★**************★********************************★*************★*************** 
FUNCTION: findSymConfigl() 

PARAMETERS: double d — distance from the origin (base configuration) to 

the symmetric configuration. 

double alpha —The angular difference between both orientations 
PURPOSE: This finds the symmetric configuration of a configuration 

described by alpha and a above. 

RETURNS: CONFIGURATION: sym_config —the symmetic configuration 

COMMENTS: Was called def_syml() in MML10 

This function overcomes the drawback of the original 
find_sym_config() of not being able to handle the situation when 
alpha equals PI 

******************************************************************************/ 
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CONFIGURATION 

findsymConfigl(double d, double alpha) 

{ 

double beta = alpha/2; 

return defineConfig(d * cos(beta), d * sin(beta), alpha, 0.0); 

} 

/******************★****★*************************★*★*★**★**************★*★**** 
FUNCTION: inverse() 

PARAMETERS: CONFIGURATION original —the original configuration 

in global coordinates 

PURPOSE: To calculate the inverse of a given configuration 

RETURNS: CONFIGURATION: the inversed configuration 

such that; 

original * inversed = Identity 

COMMENTS: None 

★★★★★★★★★★★★★★★★it*************************************************************/ 

CONFIGURATION 

inverse(CONFIGURATION original) 

{ 

CONFIGURATION inversed; 

inversed.Posit.X = -original.Posit.X * cos(original.Theta) - 
original.Posit.Y * sin(original.Theta); 

inversed.Posit.Y = original.Posit.X * sin(original.Theta) - 
original.Posit.Y * cos(original.Theta); 

inversed.Theta » -original.Theta; 
inversed.Kappa = -original.Kappa; 

return inversed; 


/a***************************************************************************** 
FUNCTION: compose() 

PARAMETERS: CONFIGURATION *first — pointer to the first configuration 

♦second — pointer to the second configuration 
PURPOSE: To calculate the composition of the first and second 

configurations 

RETURNS: CONFIGURATION: configuration which is the 

composition of the first and second configurations 
COMMENTS: A typical example of the usage of this function is to determine 

the goal position of a configuration in global coordinates. In 
such an example, the first argument would be the original 
configuration and the second argument would be the goal 
configuration in the original configuration's local coordinate 
system. The resultant third argument would then be the goal 
configuration in global coordinates.Was called comp() in MML10 
LAST UPDATE: 10/25/94 Chien-Liang Chuang 
******************************************************************************/ 
CONFIGURATION 

compose(CONFIGURATION * first, CONFIGURATION * second) 

{ 

CONFIGURATION third; 
double x,y, theta; 
double xx,yy,tt; 

x = second->Posit.X; 
y = second->Posit.Y; 
theta = first->Theta; 


xx = cos(theta) * x - sin(theta) * y + first->Posit.X; 

yy = sin(theta) * x + cos(theta) * y + first->Posit.Y; 

tt = first->Theta + second->Theta;/* no need to normalize 10/25/94 Chuang */ 
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third.Posit.X = xx; 
third.Posit.Y - yy; 
third.Theta = tt; 
third.Kappa = first->Kappa; 

return third; 

} 


/********* ** **** *** ****** ** ★ ***** *** ***** ****** ** ** ** *********** ** ** *********** 


FUNCTION: 

PARAMETERS: 


PURPOSE: 

RETURNS: 
COMMENTS: 


circularArc() 

CONFIGURATION length —the arc length 

alpha —the end orientation 

config —pointer to the resultant configuration 
Given the arc length and alpha, to calculate the final 
configuration 

CONFIGURATION: pointer to the final configuration 
The main purpose of this function is to be used in conjunction 
with compose(} to form a new next(). In this case, length would 
actually be delta-s and alpha would be delta-theta. 

Circular_arc{) would determine the configuration after the incre¬ 
mental move in the local coordinate system of the original 
configuration. Then compose() would take the original 
configuration (in global coordinates) and the incremental 
configuration (in local coordinates) to determine the 


incremental configuration in global coordinates. 
************************************************** ************************^ 
CONFIGURATION 

circularArc(double length, double alpha) 

{ 


*** ! 


double alpha2,alpha4; 

alpha2 = SQR(alpha); 
alpha4 = SQR(alpha2); 

return defineConfig((1- alpha2/6.0 + alpha4/120.0) * length, 

(0.5 - alpha2/24 + alpha4/720.0) * alpha * length, 
alpha, 0.0); 


/****************************************************************************** 
Function : orthogonalize() 

Purpose : To set the input orientation to the nearest angle of a (integer) 
multiple of PI/2 (orthogonal orientation) 

Parameters: double theta 

Returns : orthogonal orientation 

Comments: 

***********************★******************************************************/ 
static double 

orthogonalize(double theta) 

{ 

if (theta >= 0.0) 

return (((int) ((theta + QPI) / HPI)) * HPI); 
else 

return (((int) ((theta - QPI) / HPI)) * HPI); 


/****************************************************************************** 
Function : computeThetaO 

Purpose : To compute the orientation from a point to another 

Parameters: POINT pi, POINT p2 

Returns : the orientation (type double) 

Comments: 

******************************************************************.** ;jl .*** + **^**^ 
static double 

computeTheta(POINT pi, POINT p2) 

{ 
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return atan2(pl.Y - p2.Y, pl.X - p2.X); 

} 


/**********★★**************************************** 

Function : areaTriangO 

Purpose : To determine the area of any triangle. 

Parameters: Point pi the first point 

Point p2 the second point 

Point p3 the third point 

Returns : the area of triangle (type double) 

Comments : July 15, 1996 Mahmoud Wahdan 
************************ *★***★★■*■*■*•****★★*********★**/ 
static double 

areaTriang(POINT pi, POINT p2, POINT p3) 

{ 

double area; 

/* calculate the area of the triangle */ 
area = 0.5*((p2.X - pl.X) * (p3.Y - pl.Y) - 
(p3.X - pl.X) * (p2.Y - pl.Y)); 

return area; 

} 

/****************************************************************************** 
Function : depth() 

Purpose : To compute the depth of a point along a line defined by the 
orientation. 

Parameters: POINT p, double alpha — a point and it's orientation 

Returns : the depth of the point 

Comments: 

******************************************************************************y 

static double 

depth(POINT p, double alpha) 

{ 

return p.X * cos(alpha) + p.Y * sin(alpha); 


/****************************************************************************** 
Function: Psi_function() 

Purpose: Computes the Psi function of two given points 
Parameters: POINT pl,p2 
Returns: double 
Comments: 

******************************************************************************^ 
double 

Psi(POINT pi,POINT p2) 


if { p2.Y - pl.Y == 0.0 && p2.X - pl.X == 0.0) 
return 0.0; 
else 

return atan2(p2.Y - pl.Y, p2.X - pl.X); 


/****************************************************************************** 
Function: distance() 

Purpose: Computes the distance between two given points 
Parameters: POINT pl,p2 
Returns: double 
Comments: 

****************************************************************************** j 

double 

distance(POINT pi,POINT p2) 

{ 

double X, Y; 


96 




X = pl.X - p2.X; 

Y = pi.Y - p2.Y; 

return sqrt( X*X + Y*Y ); 

} 

/**************★*************************************************************** 
Function : IntersectLineToLine() 

Purpose : To compute the intersection of two lines. 

Parameters: CONFIGURATION pi, CONFIGURATION p2 — two lines 

Returns : configuration of intersection 

Comments: 

*****************************************************************+************ j 

CONFIGURATION 

intersectLineToLine(CONFIGURATION pi, CONFIGURATION p2) 

{ 

CONFIGURATION inter; 

inter.Posit.X = ( -cos(p2.Theta) * 

(pi.Posit.X * sin(pi.Theta) - pi.Posit.Y * cos(pi.Theta)) 

+ cos(pi.Theta) * 

(p2.Posit.X * sin(p2.Theta) - p2.Posit.Y * cos(p2.Theta)) 

) 

/ sin(p2.Theta - pl.Theta); 

inter.Posit.Y = ( sin(pl.Theta) * 

(p2.Posit.X * sin(p2.Theta) - p2.Posit.Y * cos(p2.Theta)) 

- sin(p2.Theta) * 

(pi.Posit.X * sin(pi.Theta) - pi.Posit.Y * cos(pi.Theta)) 

) 

/ sin(p2.Theta - pl.Theta); 

inter.Theta = pl.Theta; /* in some cases, maybe p2.t will be needed */ 
inter.Kappa = 0.0; 
return(inter); 

} 

/*★*******★**************★************★***★**********★**********■*************** 
Function : order() 

Purpose : To determine the order (i.e. CW or CCW) of three points 
Parameters: Point pi the first point 

Point p2 the second point 

Point p3 the third point 

Returns : int CW if clockwise, CCW if counterclockwise 

Comments: 

*******************************************************************1'********** j 

int 

order(POINT pi, POINT p2, POINT p3) 

{ 

double area; 

/* calculate the area of the triangle */ 
area « ((p2.X - pl.X) * (p3.Y - pl.Y) - 
(p3.X - pl.X) * (p2.Y - pl.Y)); 

if (area > 0.0) 
return (1); 
else if (area < 0.0) 
return (-1); 
else 

return (0); 
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/*★****★********************************************************************** 
File Name: motion.c 

Environment: GCC ANSI C compiler for the SPARC processor 
Description: This file provides the routines and data structures needed to 
provide the motion control capability for the robot. The file 
is divided into three sections The first is the private section 
containong the encapsulated data and functions. The second 
section is the control section. This section defines the 
routines required for motion control. The third section is the 
Immediate command section. This section defines MML*s immediate 
commands. The routines in these last two sections can be 
accessed publicly. 

Last update: May 30, 1995 Midportion MP is added (modified July,20 1995) 
June 18,1995 symmetric path tracking is added (testing) 

July 21,1995 SingleregionMP{) added, tested ok. 

July 29,1995 Tearing up the motion.c 

May, 1999 Wall following motion mode added, Dan Wells 
**★**************************★***★**************★***★***★**★**★★***★**********/ 

/♦♦define TIMER*/ 

#include "definitions.h" 

#include "pcdefs.h" 

/*#include "PCdefs.h" */ 

♦include "wheels.h" 

#include <math.h> 

♦include "queue.h" 

♦include "motionlog.h" 

♦include "geometry.h" 

♦include "stdiosys.h" 

♦include "time.h" 

♦include "system.h" 

♦include "trace.h" 

♦include "motion.c.h" 

♦include "sonar.h" 

♦include "sonarmath.h" /* Added: D Wells 4-12-99 */ 

♦include "sonarcard.h" /* Added: D Wells 5-16-99 */ 

♦include "seqcmd.h" 

♦include "memsys.h" 


/***★**********★**★******★*★*******************★********★★******★★************★ 
PRIVATE SECTION 

The following section defines the encapsulated definitions, data structures 
and prototypes used in the system. 

****************★********★★***★★**★*★★*****★********★***********★****★***★*★**/ 

/****★**♦*★★*★*★*★★★★*★★★***★★★**★★*★★*★★★★★*★★*★★★★★★**★★★★* 
constant definition 

***********★**★**************★***********★******★***•*★*****★/ 

♦define HALFTREAD 26.20623239064972878310 

♦define DEFAULT_LIN_ACC 10.0 

♦define DEFAULT_ROT_ACC 0.25 

♦define DEFAULT_GOAL_VEL_LIN 30.0 

♦define DEFAULT_GOAL_VEL_ROT 5.0 

♦define SIGMA 20.0 


♦define LEFT 1 

♦define RIGHT 0 

♦define ON 1 

♦define OFF 0 

♦define NIL -1 

♦define CW -1 

♦define COL 0 

♦define CCW 1 
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/************************************************************ 
global variables (in motion.c file) declarations 
************************************************************^ 

BOOLEAN Halted; 

static double deltas; 

static VELOCITY Commanded; ^commanded velocities */ 

static double kappaCommanded; /* commanded kappa */ 

static CONFIGURATION vehicle; /* local variable for 

vehicle configuration*/ 

static LINE currentPath; /* holds the current path element values */ 

static VELOCITY haltedVel; 

static VELOCITY goalVel, 

goalAcc; 

static double desiredSigma, 

totalDistance; 

static int LoopTest; 

static double lambdaNew, lambdaOld; 

static int decFlag = FALSE; 


/************* the following functions are for model based motion **********/ 

/*********************************************************** 

The following static function declarations are the 
prototypes for the encapsulated functions. 

************************************************************i 


/* calculates the next commanded linear velocity value. */ 
static double computeLinVelocity(double ActualVelocity, 

double LastCommandedVelocity); 

/* calles a motion rule function based on the mode of travel that the 
vehicle is in */ 

static VELOCITY motionRules(VELOCITY Actual, VELOCITY Commanded); 


/* calculates the distance remaining on a path to reach a configuration. 
Used with bline calculation. */ 
static double restOfPath(CONFIGURATION ql, LINE 11); 

/* determines whether the vehicle needs to decelerate. 

Used in bline calculation */ 
static int needsToDecelerate(double actualVelocity); 


/* determines whether it*s time to process the next instruction */ 


void 

setSteeringVar(); 

/****** Global variables for parabola tracking ******/ 
Static VELOCITY 

stopRule(VELOCITY actual, VELOCITY commanded); 
static VELOCITY 

trackRule(VELOCITY actual, VELOCITY commanded); 
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Static VELOCITY 

wallHugRule(VELOCITY actual, VELOCITY commanded); 
static VELOCITY 

rotateRule(VELOCITY actual, VELOCITY commanded); 

double steer(CONFIGURATION robot, LINE line); 
double 

steerByWall(CONFIGURATION robot, CONFIGURATION curWall, double offset); 


/a***************************************************************************** 


MOTION CONTROL SECTION 

The following section defines the functions that provide access to the 
motion control system. These routines are public. 

****************************************************************************** j 

/****************************************************************************** 
Function InitMotionO initializes all of the private global variables 
in this module to the default values. It then calls SetTimer to 
program the 5th timer on serial board #l(the second serial board)to generate 
synchronous interrupts every 10ms. After the timer has been set up, the 
interrupt handling routine is made available to the system by the call to 
SetMotionlnterruptHandler(). 

★ *★**★******★★★*★***★★+★★****★**★*********★**★***★***•**■*•*•******★***•*•**■**■*•* + •*•*•* j 


void 

InitMotion(void) 

{ 

LoopTest = 0; 

Halted = FALSE; 
haltedVel.Linear = 0.0; 
haltedvel.Rotational - 0.0; 
decFlag - FALSE; 

/* Initialize motion related systems */ 

InitSeqcmd () ; 

InitWheels (); 

/* Initializes the distance. Updated every motion control cycle by deltas*/ 
setTotalDistancelmm(O.O); 

/* Initialize the goal velocities */ 
setLinVelImm(DEFAULT_GOAL__VEL_LIN) ; 
setRotVellmm (DEFAULT_GOAL__VEL_ROT) ; 

/* Initialize the commanded velocities */ 

Commanded.Linear = 0.0; 

Commanded.Rotational = 0.0; 

/* Initalize the linear and rotational acceleration */ 
setLinAccImm (DEFAULT_LIN__ACC) ; 
setRotAccImm(DEFAULT_ROT_ACC) ; 

/* Initialize the size constant */ 
setSigmalmm(SIGMA) ; 


/* add a part of trackline */ 

/* Initialize the commanded kappa */ 
kappaCommanded = 0.0; 

/* Initialize the clearance */ 
/*setClearanceImm(CLEARANCE);*/ 

/* Initialize the vehicle configuration */ 
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vehicle.Posit.X = 0.0; 
vehicle.Posit.Y = 0.0; 
vehicle .Theta ** 0.0; 
vehicle.Kappa = 0.0; 

/* Initalize the current path configuration */ 
currentPath.Type = STOPMODE; 
lambdaOld = INF; 


/********★********************************************************★************ 
Function MotionSysControl() is the interrupt handler workhorse.lt is called 
from the assembly interrupt handler shell. Its first task is to update the 
change in position and orientation through calls to the module responsible 
for movement. It then uses this information in the motion control laws to 
derive the commanded linear and rotational velocities required for this 
motion control cycle. Finally, it passes these computed velocities back to 
the movement module for execution. 

******************************************************************* j 


void 

MotionSysControl(void) 

{ 

double deltaTheta; 

VELOCITY Actual; /* variable used to hold the actual vehicle velocity */ 

#ifdef TIMER 

int tick = getCount(); 

#endif 

UpdateMovementO; /* updates the distance traveled by both wheels—found in 
Wheels.c */ 

deltas = GetDistanceTraveled(currentPath); /*returns the linear distance the 
vehicle 

has travelled between the last two calls 
to 

UpdateMovementO—found in Wheels.c */ 

/* updated by mahmoud wahdan Nov, 14 95 


deltaTheta = GetOrientationChange(); /* returns the difference between the 

changes in the distance of the left 
and right wheels between the 
last two calls to UpdateMovement(). 
Found in Wheels.c */ 

totalDistance += deltas; /* Keeps track of the total distance traved 

by vehicle */ 

/*update the vehicle's configuration based on the distance travelled 
during the last motion control cycle */; 

vehicle = localize(vehicle, deltas, deltaTheta); 


/* next 2 lines calculate the actual velocity that robot maintained based 
on the distance travelled over the last motion control cycle. */ 

Actual.Linear = deltas / MOTION_CONTROL__CYCLE; 

Actual.Rotational = deltaTheta / MOTION_CONTROL_CYCLE; 

/* logs the values of the vehicle configuration. Data is 
written to a buffer during each motion control cycle and 
then downloaded to a file when the program ends. LogMotion 
is found in motionlog.c */ 


LogMotion(vehicle); 


/* motionRules returns the commanded velocities that will be 

used in the next motion control cycle. Found in this module.*/ 


101 



Commanded = motionRules(Actual,Commanded); 

/* SetMovement() translates the commanded linear and 

rotational velocities into commanded velocities for each 
wheel. Found in wheels.c */ 

SetMovement(Commanded.Linear,Commanded.Rotational); 

/♦transition(); reads next instruction if needed. Found in this module.*/ 

/* Increments the "time” every motion control cycle for the 
various timer functions. Found in time.c */ 

clockTick(); 

/* blinkLED is used to control output from interrupt driven 
motion control system. It turns an LED on and off every 
second. Function found in this module.*/ 

blinkLED () ; 

} 


/****************************************************************************** 
Function : setRobotConfigImm() 

Purpose : To set and update the robot configuration 
Parameters: CONFIGURATION NewConfig 
Returns : void 
Comments : 

****************************************************************************** j 

void 

setRobotConfiglmm(CONFIGURATION NewConfig) 

{ 

Disablelnterrupts(); 

vehicle.Posit.X = NewConfig.Posit.X; 
vehicle.Posit.Y = NewConfig.Posit.Y; 
vehicle.Theta = NewConfig.Theta; 
vehicle.Kappa = NewConfig.Kappa; 

Enablelnterrupts(); 

} 


/****************************************************************************** 
Function : getRobotConfig() 

Purpose : Retreives the current robot configuration 

Parameters: Pointer to a variable where the current values for the robot's 
configuration will be placed. 

Returns : void 
Comments: 

*********★********★★**★*******★★★★★***★**********★★*★★******★**★**★★********** j 

CONFIGURATION 
getRobotConfig(void) 

{ 

CONFIGURATION temp; 

Disablelnterrupts(); 
temp = vehicle; 

Enablelnterrupts(); 
return temp; 


/************************r**************************************************** 

Function : localize() 

Purpose : Calculates the next configuration of the vehicle based on the 
distance that the robot travelled during the last motion 
control cycle 
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Parameters: 


Returns 
Comments: 


CONFIGURATION robot —from the last motion control cycle 
double deltas — linear distance travelled in last 
motion control cycle 

double deltaTheta —angular change in the last motion 
control cycle 

CONFIFURATION —of the vehicle based on the distance travlled 
during the last motion control cycle 


***************************************************************************^ 


CONFIGURATION 


localize(CONFIGURATION robot, double deltas, double deltaTheta) 

{ 

CONFIGURATION tempRobot; 


tempRobot = circularArc(deltas, deltaTheta); 
robot = compose(Srobot, StempRobot); 
robot.Kappa - kappaCommanded; 

return robot; 

} 


/***************************************************************************** 
Function : computeLinVelocity() 

Purpose : Calculates the linear component of the' commanded velocity. 
Parameters: double ActualVelocity, double CommandedVelocity 
Returns : new commanded linear velocity 
Comments: 

*****************************************************************************y 

static double 

computeLinVelocity(double ActualVelocity, double CommandedVelocity) 

{ 

double VelocityChange; 

double goalSpeed; 

VelocityChange = goalAcc.Linear * MOTION__CONTROL_CYCLE; 

goalSpeed = goalVel.Linear / (1.0 + HALFTREAD * fabs(kappaCommanded)); 

if (ActualVelocity < goalSpeed) 

CommandedVelocity = Min(CommandedVelocity + VelocityChange, 
goalSpeed); 

else 

CommandedVelocity - Max(CommandedVelocity - VelocityChange, 
goalSpeed); 


return CommandedVelocity; 

} 

/***************************************************************************** 
Function : computeLinVelocityS() 

Purpose : Calculates the linear component of the commanded velocity. 
Parameters: double ActualVelocity, double CommandedVelocity 
Returns : new commanded linear velocity 
Comments: 

*****************************************************************************^ 
static double 

computeLinVelocityS(double ActualVelocity, double CommandedVelocity) 

{ 

double VelocityChange; 

double goalSpeed; 

VelocityChange « goalAcc.Linear * M0TI0NJS0NTR0L_CYCLE; 

if(decFlag |I needsToDecelerate(ActualVelocity)) 

{ 

CommandedVelocity VelocityChange; 
if(CommandedVelocity <= 0.0) 
currentPath.Type = STOPMODE; 

} 
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else 

{ 

goalSpeed « goalVel.Linear / (1.0 + HALFTREAD * fabs(kappaCommanded)); 


} 


if (ActualVelocity < goalSpeed) 

CommandedVelocity ® Min(CommandedVelocity + VelocityChange, 
goalSpeed); 

else 

CommandedVelocity = Max(CommandedVelocity - VelocityChange, 
goalSpeed); 


} 

return CommandedVelocity; 


/************★*★***********★*★********★**************************************** 
Function : restOfPath() 

Purpose : calculates the remaining distance to the ending configuration for 
Parameters: void 

Returns : the distance between current position to the STOPPING 

configuration 

Comments: 

******************************************************************************^ 
static double 

restOfPath(CONFIGURATION ql, LINE 11) 

{ 

CONFIGURATION q2 = ll.config; 
if (q2.Kappa 0.0) 

return ((q2.Posit.X - ql.Posit.X) * cos(q2.Theta) + 

(q2.Posit.Y - ql.Posit.Y) * sin(q2.Theta)); 

else 

if (q2.Kappa > 0.0) 
return (11.Radius* 

positiveNorm(Psi(11.Center,q2.Posit) - 
Psi(11.Center,ql.Posit))); 

else 

return (11.Radius* 

negativeNorm{Psi(11.Center,q2.Posit) - 
Psi(11.Center,ql.Posit))); 


/*******************************************★********************************** 
Function : needToDecelerate() 

Purpose : To determine whether the robot needs to begin decelerating. Such 
as in a bline function. 

Parameters: double actualVelocity (linear) 

Returns : It returns 1 if it needs to decelerate. Otherwise, it returns 0. 
Comments: 

*******************************************************************************y 

static int 

needsToDecelerate(double actualVelocity) 

{ 

int decelerate; 
decelerate - FALSE; 

if (2.0 * goalAcc.Linear * restOfPath(vehicle,currentPath) 

<- actualVelocity * actualVelocity) 
decelerate = TRUE; 

decFlag = decelerate; 
return decelerate; 


/****************★*******★********************************.********************* 
Function : motionRules() 

Purpose : To calculate the linear velocity and rotational velocity based 
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on the type of motion that the robot is executing. 

Parameters: VELOCITY actual, commanded 

Returns : The commanded linear and rotational velocities. 

Comments: 

****************************************************************************** j 

static VELOCITY 

motionRules(VELOCITY actual, VELOCITY commanded) 

{ 

switch (currentPath.Type) { 
case STOPMODE: 

commanded - stopRule(actual, commanded); 
break; 

case TRACKMODE: 
case TRACKSMODE: 

commanded = trackRule(actual, commanded); 
break; 

case TRACKRWALLMODE: 
case TRACKLWALLMODE: 

commanded = wallHugRule(actual, commanded); 
break; 

case ROTATEMODE: 

commanded = rotateRule(actual, commanded); 
break; 

default: 

break; 

} 

return commanded; 

} 

/****************************************************************************** 
Function : stopRule() 

Purpose : updates the commanded velocity to 0 (zero) to stop the robot 
Parameters: VELOCITY actual, commanded 

Returns : The required linear velocity, rotational velocity 
Comments: 

*************************************************************** *************** j 

static VELOCITY 

stopRule(VELOCITY actual, VELOCITY commanded) 

{ 

/*if there is a next line in the buffer, take out as the current line*/ 

commanded.Linear = 0.0; 
commanded.Rotational = 0.0; 

if(!IsBufferEmpty()) 

{ 

currentPath = GetLineO; 

} 

return commanded; 

} 

/****************************************************************************** 
Function : testNeutral() 

Purpose : to determine if the robot is at the neutral switching point 
Parameters: None 
Returns : 

Comments : 

****************************************************************************** j 

int testNeutral(LINE nextLine, double lambdaNew ) 

{ 

double angle; 

if (nextLine.config.Kappa == 0) { 
if (currentPath.config.Kappa 0) { 

angle = norm(nextLine.config.Theta - currentPath.config.Theta); 
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if {fabs(angle) < 0.001) { 
return TRUE? 

} else { 

return (angle*lambdaNew >= 0.0); 

} 

} else { 
return 0; 

} 

} else { 
return 0; 

} 

} 

/****************************************************************************** 
Function : trackRule() 

Purpose : To determine the linear and rotational velocities needed to put or 
keep Yamabico on the path. 

Parameters: VELOCITY actual, commanded 

Returns : The required linear velocity, rotational velocity 
Comments: 

★★★★★★a***********************************************************************^ 

static VELOCITY 

trackRule(VELOCITY actual, VELOCITY commanded) 

{ 

double deltaDistance, angle; 

LINE nextLine? 

if(!IsBufferEmpty()) 

{ 

nextLine = ReadNextLine(); 

lambdaNew = steer(vehicle,nextLine)? 

angle = fabs(norm(Psi(vehicle.Posit,nextLine.Center) 

- vehicle.Theta)); 

if ((lambdaOld != INF) && (lambdaNew*lambdaOld <= 0.0) 

&& ((currentPath.config.Kappa -- 0.0) 

1j (nextLine.config.Kappa == 0.0) 

|| (distance(vehicle.Posit, nextLine.Center) <= 
fabs(nextLine.Radius) * 0.9) 

| | (angle < HPI*0.9))) 

/* if (testNeutral(nextLine, lambdaNew))*/ 

{ 

currentPath = GetLine(); 
lambdaOld = INF; 

} 

else 

{ 

lambdaOld = lambdaNew; 

} 

} 

if (currentPath.Type == TRACKMODE) 

commanded.Linear = computeLinVelocity(actual.Linear,commanded.Linear); 
else 

commanded.Linear = computeLinVelocityS (actual.Linear,commanded.Linear); 

deltaDistance = MOTION_CONTROL_CYCLE * commanded.Linear; 

kappaCommanded = vehicle.Kappa + 

steer(vehicle,currentPath) * deltaDistance; 
commanded.Rotational = kappaCommanded * commanded.Linear; 

return commanded; 

} 

/a***********************************************************.*.*******.********* 
Function : wallHugRule() 

Purpose : To determine the linear and rotational velocities needed to put 
or keep Yamabico at the correct offset from the wall. 

Parameters: VELOCITY actual, commanded 
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Returns : The required linear velocity, rotational velocity 
Comments: Added by Dan Wells for wall following. May 99. 

*************************************************************************★★**/ 

/*************************★********************* j 

/**** code LISTED IN APPENDIX C OF THESIS ****/ 

/*********************************************** j 

/********************************★*****************************************★★**/ 
double OldLambdaO 

{ 

return lambdaOld; 

} 

double NewLambdaO 

{ 

return lambdaNew; 

} 

/****************★***********************★************************************* 
Function : rotateRule() 

Purpose : updates the commanded velocity to rotate the robot 
Parameters: VELOCITY actual, commanded 

Returns : The required linear velocity, rotational velocity 
Comments: last updated 10/25/94 Chien-Liang Chuang 

*********★★************************************★******************************/ 
static VELOCITY 

rotateRule{VELOCITY actual, VELOCITY commanded) 

{ 

static double goalTheta; 
static int noGoalTheta = TRUE? 

double deltaVel = goalAcc.Rotational * MOTION_CONTROL_CYCLE; 
double rotVel? 
double rotAcc; 
int Direction; 

rotVel - goalVel.Rotational; 
rotAcc = goalAcc.Rotational; 
if (noGoalTheta) 

{ /* set the goal of rotation */ 

goalTheta - vehicle.Theta + currentPath.config.Theta; 
noGoalTheta = FALSE; 

} 

commanded.Linear = 0.0; 

Direction = (currentPath.config.Theta >=0.0) ? CCW : CW; 

/* First, handle the acceleration */ 
if (2.0 * rotAcc * fabs(goalTheta - vehicle.Theta) > 
commanded.Rotational * commanded.Rotational) 

{ 

if (Direction == CCW) /* for CCW rotation */ 

commanded.Rotational = Min(commanded.Rotational + deltaVel, rotVel); 
else /* for clockwise rotation */ 

commanded.Rotational = Max(commanded.Rotational - deltaVel, -rotVel); 

} 

else 

{ /* Now, handle the deceleration */ 

if (Direction == CCW) /* for CCW rotation */ 

{ 

if (vehicle.Theta < goalTheta) 

commanded.Rotational = Max(commanded.Rotational - deltaVel, 0.0); 
else 
{ 

LEDon(4); /* turn on the LED #4 when finish the rotation */ 
currentPath.Type = STOPMODE; 
commanded.Rotational = 0.0; 
noGoalTheta = TRUE; 

} 

} 

else 
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{ /* for CW rotation */ 

if (vehicle.Theta > goalTheta) 

commanded.Rotational - Min(commanded.Rotational + deltaVel, 0.0); 
else 
{ 

LEDon(4); /* turn on the LED #4 when finish the rotation */ 
currentPath.Type = STOPMODE; 
commanded.Rotational = 0.0; 
noGoalTheta * TRUE; 

} 

} 

> 

return commanded; 

} 


/***************************************************************** + * + **^^ ++ ^^ 
Function: setLinVellmmO 

Purpose: sets and updates the linear velocity of the robot 
Parameters: double LinearVelocity 
Returns: void 
Comments: 

*****************************************************************************^ 
void 

setLinVellmm(double linearVelocity) 

{ 

goalVel.Linear = linearVelocity; 

} 


/*************************************************+********+******************* 
Function: setRoVellmmO 

Purpose: Sets and updates the rotational velocity 
Parameters: double RotationalVelocity 
Returns: void 
Comments: 

******************************************************************************i 

void 

setRotVelImm(double RotationalVelocity) 
goalVel.Rotational = RotationalVelocity; 


/******★**★★★**■*■*★★★★********************************************************** 
Function: setLinAccImm() 

Purpose: Sets and updates the linear acceleration 

Parameters: double LinearAcceleration 
Returns: void 
Comments: 

******************************************************************************i 

void 

setLinAccImm(double LinearAcceleration) 

{ 

goalAcc.Linear = LinearAcceleration; 

} 


/***********************************************************^ Mk .* -A .^ Mt+ + + ^. A .^^^ A + ^ + + 
Function: setRotAccImm() 

Purpose: Sets and updates the rotational acceleration 
Parameters: double RotationalAcceleration 
Returns: void 
Comments: 

****************************************************************** <A> + ^.^.^^, i ^ <i ^.^, + + y 

void 

setRotAccImm(double RotationalAcceleration) 
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goalAcc.Rotational - RotationalAcceleration; 

} 

/****************************************************************************** 
Function: setTotalDistancelmmO 

Purpose: sets the total distance travelled by the robot to the value passed 
as a parameter 
Parameters: double distance 
Returns: void 
Comments: 

******************************************************************************/ 

void 

setTotalDistanceIiran (double distance) 

{ 

totalDistance = distance; 


/****************************************************************************** 
Function: getTotalDistancelmmO 

Purpose: returns the total distance travelled by the robot 

Parameters: void 

Returns: double totalDistance 

Comments: 

******************************************************************************i 

double 

getTotalDistancelram (void) 

{ 

return totalDistance; 

> 


y****************************************************************************** 
Function: haltMotionlmm() 

Purpose: brings the robot to a rest. Is different from stop in that it's 
original velocity is saved to be later used by the resume 
command to allow the robot to continue travelling at the same 
speed. 

Parameters: void 
Returns: void 
Comments: 

******************************************************************************^ 
void 

haltMotionlmm(void) 

{ 

if (!Halted) { 

Halted = TRUE; 

haltedVel.Linear = goalVel.Linear; 
haltedVel.Rotational = goalVel.Rotational; 

WheelsDisable(); 

} 

} 

/****************************************************************************** 
Function: resumeMotionlmm{) 

Purpose: Allows the robot to resume the speed it was travelling before the 
haltMotionlmm() command was given. 

Parameters: void 
Returns: void 
Comments: 

******************************************************************************i 

void 

resumeMotionlmm(void) 

< 

if (Halted) { 

Halted = FALSE; 

goalVel.Linear - haltedVel.Linear; 
goalVel.Rotational = haltedVel.Rotational; 
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WheelsEnable(); 

} 

} 

/***★***★*****************************★***********★************************* 
Function : blinkLED() 

Purpose : To control the output from the interrupt driven motion control 
system. LoopTest is assigned zero every second. 

Parameters: void 
Returns : none 
Comments: 

***************************************************************************y 

void 

blinkLED(void) 

{ 

if (LoopTest++ >= ( (int) (<1.0/MOTIONJOONTROL_CYCLE) - 1))) { 

changeLEDstate(7); 

LoopTest = 0; 

} 


^****************************************************************************** 
Function: stopImmO 

Purpose: updates the goal velocity to zero inorder to stop the robot 
Parameters: void 
Returns: void 

Comments: This is the immediate stop command 

*★*★★***★★*****★****************************♦*********************************/ 
void 

stoplmm(void) 

{ 


WheelsDisable(); 
goalVel.Linear = 0.0; 
goalVel.Rotational « 0.0; 

currentPath.Type = STOPMODE; /^update the path element in motion.c */ 


} 

/******★*********************************************************************** 
Function : waitMotionEnd() 

Purpose : To wait until the motions specified in user program end. 
Parameters: none 
Returns : void 

Comments: If the conditions are not satisfied, then it stays in busy waiting 

******************************************************************************/ 

void 

waitMotionEnd() 

{ 

while (!(IsBufferEmptyO && (currentPath.Type == STOPMODE))); 

} 

/****************************************************************************** 
Function: setSigmalmmO 

Purpose: sets the size constant which influences the sensitivity of the 
steering function 
Parameters: double sigmaln 
Returns: void 

Comments: last updated 10/25/94 Chien-Liang Chuang 

****************************************************************************** j 

void 

setSigmalmm(double sigmaln) 

{ 

desiredSigma - sigmaln; 
return; 
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/****************************************************************************** 
Function: setSigmaImm() 

Purpose: returns the current value of desiredSigma 
Parameters: void 
Returns: double 
Comments: 

******************************************************************************^ 
double 

sigmaValue() 

{ 

return desiredSigma; 

} 


/****************************************************************************** 
Function : setPathElement <) 

Purpose : To set the current path for the robot 
Parameters: LINE newPath 
Returns : void 

Comments : function added by D. Wells 5/5/99 for wall following motion mode 

******************************************************************************^ 
void 

setPathElement(LINE newPath) 

{ 

currentPath * newPath; 

} 

/*ADDED used by wheels.c*/ 

/***************************************************************************** 
Function : getPathElement() 

Purpose : retrieves the current path element in the motion control module 
Parameters: void 
Returns : PATH_ELEMENT 
Comments : 

*****************************************************************************^ 
LINE 

getPathElement(void) 

{ 

return currentPath; 

> 


double VelocityLinear() 

{ 

return Commanded.Linear; 
} 


/******************************************************************** 
FUNCTION : steer(robot,line) PURPOSE : evaluate steering 
function 

*********************************************************************i 

double steer(CONFIGURATION robot, LINE line) 


double lambda, angle, dist; 

if (line.config.Kappa ==0.0) 

lambda = - line.a * robot.Kappa 

- line.b * norm(robot.Theta - line.config.Theta) 

- line.c *(-(robot.Posit.X - line.config.Posit.X) * sin(line.config.Theta) 

+(robot.Posit.Y - line.config.Posit.Y) * 

cos(line.config.Theta)); 
else 
{ 

angle = Psi(robot.Posit, line.Center); 
dist = distance(line.Center, robot.Posit); 
if (line.config.Kappa >0.0) 


in 


{ 

lambda = - line.a * (robot.Kappa-line.config.Kappa) 

- line.b * norm(robot.Theta-(angle-HPI)) 

- line.c * (line.Radius - dist); 

> 

else 

lambda = - line.a * (robot.Kappa-line.config.Kappa) 

- line.b * norm(robot.Theta-(angle+HPI)) 

- line.c * (line.Radius + dist); 

} 

return lambda; 


/* This function is grabbing the currentPath before the turn. 
We will make more tests later.*/ 

double waitOnTrack(CONFIGURATION path) 

{ 

double lambda = 20.0; 

CONFIGURATION robot1; 

double kk; 
double k, kk2; 

LINE pathElement; 

double sigma « sigmaValue{); 

pathElement.config = path; 

kk - path.Kappa ; 

kk2 = kk * kk; 

k = 1.0/sigma ; 

pathElement.a * 3.0*k ; 

pathElement.b = 3.0*k*k - kk2 ; 

pathElement.c = k*k*k - 3.0*k*kk2 ; 

do { 

robotl = getRobotConfig(); 

lambda = fabs(pathElement.a * robotl.Kappa) 

+ fabs(pathElement.b * 

norm(robotl.Theta - pathElement.config.Theta)) 

+ fabs(pathElement.c * 

(-(robotl.Posit.X - pathElement.config.Posit.X) 

* sin(pathElement.config.Theta) 

+ (robotl.Posit.Y - pathElement.config.Posit.Y) 

* cos(pathElement.config.Theta))); 

} while(lambda > 0.05); 

return lambda; 


/****★★***★★******★**★★★*★****************★**★★★**★★*****★★******************** 
Function: steerByWall(CONFIGURATION, CONFIGURATION, double) 

Purpose: Computes and returns the steering function value for the 
linear fitted (with decay) wall segment 
tracking based on the current state of the robot 
Parameters: CONFIGURATION robot — current robot config 

CONFIGURATION curWall — current linear fitted data for wall 
double offset — the amount of desired offset from the wall 
(+ for right wall, - for left wall) 

Returns: double — The wall following steering function's value 
Comments: Added by D Wells 4-12-99 

★*************★★*******★*****★**★****★**★****★**★***★★★********★***★**********/ 

double 

steerByWall(CONFIGURATION robot, CONFIGURATION curWall, 
double offset) 

{ 

double J, deltaTheta, deltaD; 
double k*(l.0/sigmaValue()); 
double a, b, c; 

a - 3.0*k; 
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b = 3.0*k*k; 
c = k*k*k; 


deltaTheta = robot.Theta - curWall.Theta; 

deltaD = signedDist(robot.Posit, curWall) - offset; 
J - -a*robot.Kappa 

-b*norm( deltaTheta) 

-c*deltaD; 

return(J); 


} 


y*********'*********^'**********************'***'******’*******'********'*’**'****'****** 
File name: queue.c 

*********************************************************** 4 '******************/ 

/* 

This file implements the (sequential) instruction buffer. All data 
is maintained locally, so all access is done through function calls. 

Tue 01-25-94 Frank Kelbe - rewrote entire buffer to redefine the interface 
and access 

Wed 01-26-94 removed all references to no_o_paths and skip_flag. "Skipping" 
has now been redefined to mean "flush the buffer as soon as a new 
instruction is ready to be added", and now becomes a pseudo-immediate 
command. 

Sun 05-15-94 Cleaned up for inclusion in mmlll. Peek... function will need 
to be rewritten when the leaving point calculations are added in. 

Fri 07-22-94 FK - modified for mmlll and new leaving point calculations. 

Need to be able to always find the next path segment instruction once 
a new instruction is processed. 

Mon 08-22-94 FK - fixed routines in Processlnstruction for tracking the 
last path segment 

Wed 08-24-94 FK - fixed NextSegment calculations. Now NextSegment is only 
looked for when the current instruction catches up. 

*/ 

#include "definitions.h" 
tinclude "system.h" 
tinclude "memory.h" 

#include "queue.h" 


/*** Local types ***/ 

typedef struct inst_element { 

LINE path; 

ProcessFunctionPtr Process; /* the callback function pointer */ 
} Instruction; 


/*** Local variables ***/ 

static 

int 

FlushBufferFlag; 

static 

int 

LineCount; 

static 

LINE 

Buffer[INST MAX] 

static 

LINE 

*GetPtr, 

*PutPtr, 

*HeadPtr, 

*LastPtr, 

♦NextSegment; 

/*** Local Prototypes 

*** j 
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static void findNextSegment (void); 

static void IncPtr(LINE **); 


/*** Code ***/ 

/***★*****★•*•★★★* ★**★★★*■**★*★★■*•**********★**★*******★★★**★★ * ★★***★* ★ ★ ★ *★★** * *** * 
PURPOSE: initialize the instruction buffer 

PARAMETERS: none 
RETURNS: void 

COMMENTS: Wed 01-19-94 Frank E. Kelbe 

This should only be called one time for initialization 
****■*■*★ ************************************************************************ ^ 


void 

InitQueue(void) 

{ 

HeadPtr = &Buffer[0]; 

LastPtr = &Buffer [INST_MAX-1]; 

FlushBuffer(); 

} 


/★★★♦★★★★★★★★♦★a*************************************************************** 

PURPOSE: Unconditionally flush the instruction buffer 

PARAMETERS: none 
RETURNS: void 

COMMENTS: Wed 01-19-94 Frank E. Kelbe 

******************************************************************************y 


void 

FlushBuffer(void) 

{ 

PutPtr = GetPtr = HeadPtr; /* reset to the start of the buffer */ 
LineCount =0; 

FlushBufferFlag = 0; 

NextSegment = NULL; 

} 


/***★*******************★****************************************************** 
PURPOSE: Used to check the status of the buffer 

PARAMETERS: none 

RETURNS: 1 if the buffer is empty, 0 otherwise 

COMMENTS: Wed 01-19-94 Frank E. Kelbe 

******************************************************************************^ 


int 

IsBufferEmpty(void) 

{ 

return (PutPtr ~ GetPtr); 

} 

/*****★★*******★**********★*********★******************************************* 
PURPOSE: Effectively causes the buffer to be flushed once a new 

instruction arrives 
PARAMETERS: none 
RETURNS: void 

COMMENTS: Wed 01-26-94 Frank Kelbe replaces skip() 

********************************************************************************^ 
/* allows the queue to remember to flush the buffer as soon as the next 
instruction is received and ready to be added */ 
void 

SkipAllLines(void) 

{ 

FlushBufferFlag = 1; 

} 
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/***********************★******************************************************* 

PURPOSE: Flushes the buffer, then adds the next instruction 

PARAMETERS: none 

RETURNS: void 

COMMENTS: Wed 01-26-94 Frank Kelbe 

*******************************************************************************i 

void 

RestartLine(LINE path, ProcessFunctionPtr Process) 

{ 

FlushBufferFlag = 1; 

AddLine(path); 

} 


/*********★*********★******************★*★**********************★*****★******** 
PURPOSE: see the next segment to be executed. 

PARAMETERS: a pointer to the PATHJ2LEMENT to be filled in 
RETURNS: a 1 if there is a path, otherwise a 0 

COMMENTS: Fri 07-22-94 Frank Kelbe 

revised Sun 08-21-94 

******************************************************************************j 

int 

PeekNextSegment(LINE *path) 

{ 

if (NextSegment) { 

*path = *NextSegment; 
return 1; 

} 

else 

return 0; 


/****************************************************************************** 

PURPOSE: set instruction onto the buffer 

PARAMETERS: path of type PATH ELEMENT 

RETURNS: void 

COMMENTS: Wed 01-19-94 Frank Kelbe 

******************************************************************************^ 
void 

AddLine(LINE path) ' 

{ 

/* while (LineCount >= INST_MAX)*/ /* wait for room in the buffer */ 

/* ; NULL statement */ 

if (FlushBufferFlag) /* flush the buffer now that we have a new */ 
FlushBuffer(); /* instruction to add */ 

/* Disablelnterrupts();*/ 

*PutPtr = path; /* save the path and callback function */ 

IncPtr(SPutPtr); 

/* LineCount++;*/ 

/* Enable Interrupts ()•;*/ 

> 


/***************************************************************** + ******* + ^ 

PURPOSE: GetLine 

PARAMETERS: none 

RETURNS: void 

COMMENTS: May-97 
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*★*★★★★**★****★********•*★ ★★******★**■*★**■*.******•*******■*.* + ***** *^ 

LINE 

GetLine(void) 

{ 

LINE new; 

new = *GetPtr; 

IncPtr(&GetPtr); 

return new; 

j./***********★***************★*************************★********************** 

PURPOSE: ReadLine 
PARAMETERS: none 
RETURNS: void 

COMMENTS: May-97 

***************************************************************y 

LINE 

ReadNextLine(void) 

{ 

LINE new; 
new = *GetPtr; 
return new; 

} 


PURPOSE: processes the next sequential instruction from 

the instruction buffer. 

PARAMETERS: none 

RETURNS: void 

COMMENTS: Thu 01-20-94 Frank Kelbe 

★♦★★★★★★★★★★a***********************************************.*** i 

/* 

All of the callback functions return an int, which mimics the 
use of flgjmove in the old code. If flgjnove was set ON, the 
functions return 1, otherwise they return 0. A flag move of 
OFF (return of 0) indicates to this function to keep on 
processing instructions. If an ON is returned, processing 
stops 
V 

void 

ProcessLine(void) 

{ 

int Stopprocessing = FALSE; 

LINE *last; 

while (!IsBufferEmpty() && !Stopprocessing) 

{ 

last = GetPtr; 

/* perform the callback passing a copy of the data 
to the processing function 

Stopprocessing = (*(GetPtr->Process))(GetPtr->path);*/ 

IncPtr(&GetPtr); 

LineCount—; 

> 

if (last == NextSegment || ‘NextSegment) 

/* Set the file variable to the next segment in the queue */ 
findNextSegment{); 
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/****★******★**************** 

Private Functions 

**************************** j 

static void 
findNextSegment(void) 

{ 

LINE *ptr; 

NextSegment = NULL; /* assume we can’t find another path */ 

/* No need to look for the next segment if there are no instructions */ 
if ({LineCount <= 0) || IsBufferEmpty{)) 
return; 

ptr - GetPtr; /* start looking at the next instruction */ 

/* keep looking until there are no more instructions. The checking 
is done for all elements in the queue, so the last one found is 
the "next segment". 

V 

while (ptr != PutPtr) { 

/* !*! This check may have to modified depending on how a 
"next segment" is defined */ 

if ( ptr && { 

(ptr->Type == TRACKMODE) 

)) 

{ 

NextSegment - ptr; 
break; 

) 

IncPtr(&ptr) ; 

} 

} 


static void 
IncPtr(LINE **ptr) 

{ 

++*ptr; 

if (*ptr > LastPtr) 
*ptr = HeadPtr; 

} 


/****************************************************************************** 
File name: seqcmd.c 

Descriptions: collection of all of the sequential commands that are 
available to Yamabico 
Revision history: 

******************************************************************************/ 


#include 
#include 
#include 
#include 
#include 
#include 
tinclude 
tinclude 
#include 
#include 
#include 


"definitions.h” 

"queue.h" 

"seqcmd.h" 

"motion.h" 

"time.h" 

"stdiosys.h" 

<math.h> 

"geometry.h" /* Added: D Wells 4-13-99 
"sonar.h" /* Added: D Wells 4-22-99 */ 
"sonarcard.h” /* Added: D Wells 4-19-99 
"sonarmath.h" /* Added: D Wells 4-19-99 


*/ 

*/ 

*/ 


/* local variables */ 

static MODE EndStatuslnUser; /* used to record the end status of an instruction 
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when it is added to buffer in user program */ 


void 

InitSeqcmd(void) 

{ 

EndStatusInUser *= STOPMODE; 

} 


void 

track(CONFIGURATION lineConfig) 

{ 

double kk; 
double k, kk2; 

LINE pathElement; 

double sigma = sigmaValue 0; 

pathElement.Type = TRACKMODE; 
pathElement.config = lineConfig; 
kk = lineConfig.Kappa ; 

kk2 - kk * kk; 
k = 1.0/sigma ; 
pathElement.a = 3.0*k ; 
pathElement.b - 3.0*k*k - kk2 ; 
pathElement.c = k*k*k - 3.0*k*kk2 ; 
if (kk — 0.0) 

{ 

pathElement.Radius = 0.0; 
pathElement.Center.X = 0.0; 
pathElement.Center.Y = 0.0; 

} 

else 

{ 

pathElement.Radius = 1.0/kk; 
pathElement.Center.X - lineConfig.Posit.X - 
pathElement.Radius * sin(lineConfig.Theta); 

pathElement.Center.Y = lineConfig.Posit.Y + 
pathElement.Radius * cos(lineConfig.Theta); 

} 

AddLine(pathElement); 
return; 


void 

tracks(CONFIGURATION lineConfig) 

{ 

double kk; 
double k, kk2; 

LINE pathElement; 

double sigma = sigmaValue(); 

pathElement.Type = TRACKSMODE; 
pathElement.config - lineConfig; 
kk = lineConfig.Kappa ; 

kk2 - kk * kk; 
k = 1.0/sigma ; 
pathElement.a = 3.0*k ; 
pathElement.b - 3.0*k*k - kk2 ; 
pathElement.c * k*k*k - 3.0*k*kk2 ; 
if (kk == 0.0) 

{ 

pathElement.Radius = 0.0; 
pathElement.Center.X = 0.0; 
pathElement.Center.Y = 0.0; 

} 

else 
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{ 

pathElement.Radius = 1.0/kk; 
pathElement.Center.X * lineConfig.Posit.X - 
pathElement.Radius * sin(lineConfig.Theta); 
pathElement.Center.Y - lineConfig.Posit.Y + 
pathElement.Radius * cos(lineConfig.Theta); 

} 

AddLine(pathElement); 
return; 

} 

/************ 

void 

trackWall(int sonarNum) 

{ 

double k; 

LINE pathElement; 

double sigma * sigmaValue(); 

double offset; 

/* sonarNum determines side of wall following */ 

/* Constants defined in sonar.h */ 

/* S090 - wall on right side, S270 - wall on left side * 
switch (sonarNum) { 
case S090: 

pathElement.Type = TRACKRWALLMODE; 

Offset * DEFAULT_WALLJDFFSET; 
break; 
case S270: 

pathElement.Type = TRACKLWALLMODE; 
offset - - DEFAULT_WALL__OFFSET; 
break; 
default: 

printf("\nERROR: illegal value for sonarNum in trackWall()"); 

exit(1); 

break; 

} 

pathElement.config » defineConfig(0.0, offset, 0.0, 0.0); 

k - 1.0/sigma ; 
pathElement.a = 3.0*k ; 
pathElement.b = 3.0*k*k; 
pathElement.c = k*k*k; 
pathElement.Radius = offset; 
pathElement.Center.X - 0.0; 
pathElement.Center.Y = 0.0; 

AddLine(pathElement); 

EnableSonar(sonarNum); 

EnableLinearFittingDecay(sonarNum); 

return; 

} 

*********** f 


/****************************************************************************** 
FUNCTION: Rotate (sequential) 

PURPOSE: Rotate the robot by Theta radians. 

Positive is counterclockwise and negative is clockwise. 

******************************************************************************j 

void 

Rotate(double Theta) 

{ 

LINE path; 

if (EndStatusInUser != STOPMODE) 

/* do nothing if it follows a motion without stop*/ 
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return 


path.Type = ROTATEMODE? 

/* here is where the union would be cleaner */ 
path.config.Theta = Theta; 

EndStatusInUser * STOPMODE; 

AddLine(path); 


#ifdef CODE_ISJX)NE 

/****************************************************************************** 
FUNCTION: set_error sequential 

PARAMETERS: code 

PURPOSE: set an error code into the instruction buffer. 

RETURNS: void 

CALLED BY: rotate, solve, switch_dir, set_rob 
CALLS: set_inst(); 

COMMENTS: 7 Jan 93 - Dave MacPherson 

Tue 01-25-94 Frank Kelbe - modified to use 

new buffer 
TASK: Level 0 

******* ***************************************************** ****************★*/ 
void 

set_error(ERROR_CODE code) 

{ 

PATH_ELEMENT path? 

path.mode - ERRORMODE; 
path.type = code; 

AddLine(path); 

} 


int 

ErrorProcess(PATH_ELEMENT path) 
{ 

DispError(path.type); 
halt(); 

change_status(SERROR); 
return 1; 

} 


/********************************** ************************************ 
NAME : switch_dir 

ARGUMENTS : none 

FUNCTION : to reverse the heading direction of the robot 
COMMENTS: Tue 01-25-94 Frank Kelbe - modified to use new buffer 

***************************************************************★*****★*/ 
void 

switch_dir(void) 

{ 

PATH_ELEMENT path? 

if (seq^status != SSTOP) { 

set__error (ECODE2) ; 
return; 

} 

path.type - SWITCH; 

path.pathType.mode = STOPMODE? 

AddLine(path); 

nom_p->t * norm(nom_jp->t + PI); 
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} 


/*****************************************************★***************** 
NAME : set^rob 

ARGUMENTS : Configuration to set robot’s location to. 

FUNCTION : to add set robot sequence to queue 

COMMENTS: Tue 01-25-94 Frank Kelbe - modified to use new buffer 

***********************************************************************/ 
void 

set^rob(CONFIGURATION *pst) 

{ 

PATH_ELEMENT path; 

if(seq_status !~ SSTOP) { 
set_error(ECODE2); 
return; 

} 

path.pc « *pst; 
vehicle.x = pst->x; 
vehicle.y = pst->y; 
vehicle.t = pst->t; 
vehicle.k = pst->k; 
nomjp->x - pst->x; 
nom__p- > y * pst->y; 
nom__p->t = pst->t; 
nom_p->k - pst->k; 
path.type * SET_ROB; 
path.pathType.mode = STOPMODE; 

AddLine(path); 

last_robot_path_element.pc = *pst; 
las t_robot_path_element. type = SET_ROB; 


#endif 


/*******************************^******************************* 
FILENAME: sonar.h 

PURPOSE : include file for sonar. 

AUTHOR : 

DATE : 

COMMENTS: cleaned and documented by Mahmoud Wahdan and 

Data structure LINE_SEG is removed and added 
to SEGMENT and SEGMENT changed to SEGMENT_RES & 
mOO is added to the new data structure. CUR_DATA 
is changed to SEGMENT_WORK. 

*************************************************************** j 

#ifndef _SONAR_H 

#define _SONAR_H 

#include "constants.h" 
tinclude "definitions.h" 


#define NUM_SONARS 16 

/* Sonar locations */ 



/*- 


#define 

sooo 

0 

#define 

S030 

3 

#define 

S060 

10 

#define 

S090 

7 
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tdefine S120 6 

#define S150 9 

#define S180 2 

#define S210 1 

#define S240 8 

#define S270 5 

#define S300 4 

#define S330 11 


/* Types of sonar logging */ 
/* -*/ 


#define 

SONAR NONE 

0x00 

#define 

SONAR RAW 

0x01 

tdefine 

SONAR GLOBAL 

0x02 

#define 

SONAR SEGMENT 

0x04 

#define 

SONAR_ALL 

0x07 

#define 

SONAR_CTL 

0xfc0083f9 


/* The data structure SONARD */ 
/*-*/ 


typedef struct { 


int 

fitting; 

/* 

flag to indicate linear fitting request 

V 

int 

decayFitting; 

/* 

flag to indicate linear fitting w/ decay 

*/ 

int 

updater- 

/* 

flag to indicate presence of new data 

*/ 

POINT 

global; 

/* 

global position of sonar return 

*/ 

double 

dr- 

/* 

range data 

*/ 

double 

dO; 

/* 

old range data 

V 

POINT 

posit; 

/* 

robot's position at time of range 

*/ 

double 

t; 

/* 

robot's orientation angle at time of range 

*/ 

POINT 

SonarPosit; 

/* 

position of sonar from center 

*/ 

double 

SONARD; 

SonarTheta; 

/* 

angle of sonar from robot center 

*/ 


/* defines a basic segment with the start and end points, */ 
/* the sonar number associats and the length of segment */ 
/*-*/ 

/* modified by Mahmoud Wahdan on 03-06-95 */ 

/* - ★/ 

typedef struct { 


int 

SonarNumber; 

/* 

the sonar no associates with a segment 

*/ 

double 

mOOr- 

/* 

# points of least squares LF 

*/ 

POINT 

start; 

/* 

segment with the start and end points 

*/ 

POINT 

end; 

/* 

headx, heady, tailx, taily 

*/ 

double 

alpha; 

/* 

angle and length of normal from origin 

*/ 

double 

r; 

/* 

to the segment 

*/ 

double 

SEGMENT 

length; 

RES; 

/* 

length of the segment 

*/ 


/* defines a basic segment defined by linear fitting, */ 


/* with a decay factor */ 

/*-v 

/* modified by Dan Wells on 04-15-99 */ 

/★ - */ 


typedef struct { 

int SonarNuraber; /* the sonar no associates with a segment */ 

int usable; /* flag to indicate at least 2 data points */ 
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double numPoints; /* # points of least squares LF w/ decay */ 

double alpha; /* angle and length of normal from origin */ 

double r; /* to the segment */ 

double decayFactor; /* the decay factor (range: -0 -> 1) */ 

/* 1 => no decay, all points equal weight */ 

/* ~o => previous points have no weight */ 

double mOO, /* moments of linear fitting algorithm */ 


mlO, 

mOl, 

mil, 

m20, 

m02; 

} SEGMENT_RES_DECAY; 

/* modified by Mark Merrell on 1-11-99 */ 
/*-*/ 

typedef struct { 


int 

SonarNumber; 

/* 

the sonar no associates with a 

segment 

*/ 

double 

mOO; 

/* 

# points of least squares LF 

*/ 

POINT 

start; 

/* 

segment with the start and 

end 

points 

*/ 

POINT 

end; 

/* 

headx, heady, tailx, taily 


V 

double 

alpha; 

/* 

angle and length of normal 

from origin 

*/ 

double 

r; 

/* 

to the segment 



*/ 

double 

length; 

/* 

length of the segment 



*/ 

ARRAY RES; 







/* revised by Y. Kanayama, 07-07-93 */ 

/* - */ 

typedef struct { 

double mOO, /* moments of least squares fitting algotithm */ 

mlO, ' 

mOl, 
m20, 
mil, 
m02; 

SEGMENT_RES seg; /* sonar no., startx, starty, endx, endy, */ 

/* alpha, r, length */ 

} SEGMENT WORK; 


/* Global variables */ 

/* - */ 

extern SONARD sonar_table[]; 

extern ARRAY_RES SegmentArray[NUM_SONARS][100]; 

/* Prototype */ 

/*-*/ 

void InitSonar(void); 


/* Interrupt handler */ 

/*-*/ 

void SonarSysControl(void); 

tendif 


/**********************1'*i'*ie***i'***i'*±****1tir*-k1'i(ie****ie*****i'’ki'-ki'*ir***4ici' 

FILENAME : sonar.c 

PURPOSE : Provides the global generic sonar functions 

CONTAINS : InitSonar() 
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SonarSysControl{) 


AUTHOR : Patrick Byrne, Yutaka Kanayama 

DATE : 20 November 1993 

COMMENTS : Fri 07-22-94 Updated for Sparc ntmlll FEK 

: updated by Khaled morsy 11-22-94 

: cleaned and documented on Mon 3-6-96 by Mahmoud Wahdan 
**************************★****★**★**★****★******★****★***★★★****★**★★*★/ 


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
tinclude 
#include 


"definitions.h" 
"memsys.h" 
"sonar.h" 
"motion.h" 
"sonarcard.h" 
"sonarmath.h" 
"sonarlog.h" 
"spare.h" 
"time.h" 

"trace.h" 


#ifdef TIMER 
tinclude "trace.h" 
#include "clocktick.h" 


#define TIMER_FILE 
tdefine TIMERJ5IZE 
#define TIMER_FREQUENCY 


"timer.log" 

0x10000 

1 


IOhandle TimerLog; 
#endif 


/* Global variables */ 
/*-*/ 


SONARD sonar_table[NUM_SONARS]; /* SONARD struc & NUM_SONARS = 16 */ 

/* are defined in sonar.h */ 

ARRAY_RES SegmentArray[NUM_SONARS] [100]; 

/* used by ServeSonar */ 

/* - */ 

static const int group_array[4][4] - /* array maps sonar no to groups */ 

{ 

{0, 5, 2, 7}, 

{3, 4, 1, 6}, 

{10, 11, 8, 9}, 

(12, 13, 14, 15} 


/★*•★**★*★*★★★****★*****★********★*★*★★****★★*******★★**★★★★★★*★★★★★***★****★★** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 


CALLED BY 
COMMENTS 


InitSonar() 

None 

initializes sonar table and sets up compensation for sonar 

configurations. 

void 

InitSonarmath() in sonarmath.h. 

InitSonarlog() in sonarlog.h. 

SetSonarParameters() in sonarmath.h. 
memset () in memsys.h. 

None 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 


★ *********★**★★*★*★**★**★★**★********★****★★****★★*****★*■*■★★*****★■*■****★★★★*■★** 


/ 


void 

InitSonar(void) 

{ 

int i; 
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/* initialize sonar^table */ 
/*-*/ 


for (i = 0; i < NUM_SONARS; i++) /* NUM_SONARS = 16 is defined in sonar.h */ 

{ 

memset(&sonar_table[i], 0, sizeof(SONARD)); 
sonar_table[i].fitting - 0; 
sonar_table[i].decayFitting = 0; 
sonar_table[i].update - 0; 
sonar_table[i].d = 0.0; 

sonar_table[i].d0 = 0.0; 

sonar_table[i].global.X = 0.0; 
sonar_table[i].global.Y = 0.0; 
sonarjtable[ij.t = 0.0; 

sonar_table[i].posit.X = 0.0; 
sonar_table[i].posit.Y = 0.0; 

> 

/* set up compensation for new sonar configurations 12/08/94 */ 

/*-*/ 


sonar_table[0].SonarTheta = 0.0; 
sonar_table[1].SonarTheta = 5.0*PI/6.0; 
sonar_table[2].SonarTheta = PI; 
sonar_table[3] .SonarTheta = -PI/6.0; 
sonar_table[4].SonarTheta = PI/3.0; 
sonar_table[5].SonarTheta = PI/2.0; 
sonar_table[6].SonarTheta - -2.0*Pl/3.0; 
sonar_table[7].SonarTheta « -PI/2.0; 
sonar_table[8].SonarTheta = 2.0*PI/3.0; 
sonar_table[9].SonarTheta = -5.0*PI/6.0; 
sonar_table[10].SonarTheta * -PI/3.0; 
sonar_table[11].SonarTheta = PI/6.0; 
sonar_table[12].SonarTheta = 0.0; 
sonar_table[13].SonarTheta = 1.5708; 
sonar_table[14].SonarTheta = 4.7124; 
sonar_table[15].SonarTheta = 0.0; 

sonar_table[0].SonarPosit.X - 23.6; 
sonar_table[1].SonarPosit.X = -23.0; 
sonar_table[2].SonarPosit.X = -22.6; 
sonar jtable[3].SonarPosit.X = 24.7; 
sonar_table[4].SonarPosit.X - 13.4; 
sonar_table[5].SonarPosit.X = 0.0; 
sonar_table[6].SonarPosit.X = -12.6; 
sonar_table[7].SonarPosit.X = 0.0; 
sonar_table[8].SonarPosit.X - -13.4; 
sonar_table[9].SonarPosit.X - -23.5; 
sonar_table[10].SonarPosit.X = 12.1; 
sonar__table[11] .SonarPosit.X = 25.2; 
sonar jtable[12].SonarPosit.X = 0.0; 
sonar_table[13].SonarPosit.X = 1.5708; 
sonar_table[14].SonarPosit.X = 4.7124; 
sonar_table[15].SonarPosit.X - 0.0; 

sonar_table[0].SonarPosit.Y = -0.5; 
sonar_table[1].SonarPosit.Y = 13.1; 
sonar_table[2].SonarPosit.Y = -1.0; 
sonar_table[3].SonarPosit.Y = -14.6; 
sonar_table[4].SonarPosit.Y - 21.3; 

sonar_table[5].SonarPosit.Y = 20.6; 
sonar_table{6].SonarPosit.Y = -21.3; 
sonar_table[7].SonarPosit.Y = -20.5; 
sonar_table[8].SonarPosit.Y = 21.3; 
sonar_table[9].SonarPosit.Y = -14.9; 
sonar_table[10].SonarPosit.Y = -21.3; 
sonar_table[11].SonarPosit.Y - 14.1; 
sonar_table[12].SonarPosit.Y = 0.0; 
sonar_table[13].SonarPosit.Y = 21.5; 
sonar_table[14].SonarPosit.Y = 21.5; 
sonar_table[15].SonarPosit.Y = 0.0; 
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/* initialize the sonar components */ 
/*-*/ 


InitSonarmath(); /* it is defined in sonarmath.h */ 

InitSonarlog(); /* it is defined in sonarlog.h */ 

SetSonarParameters(0.02, 5.0); /* it is defined in sonarmath.h */ 

#ifdef TIMER 

TimerLog = IOopen(TIMER_FILE, TIMER SIZE, TIMER FREQUENCY); 

#endif 


} 


/************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 

data 


SonarSysControl() 

None 

It is the ''central command* 1 for the control of all sonar related 
functions. It is linked with the ih sonar routine and loads sonar 


: to the sonar_table to determine which activities the user wishes to 
: take place, and calls the appropriate functions. This procedure is 
: invoked approximately every thirty msec by an interrupt from the 

sonar 


RETURNS 

CALLS 


CALLED BY 
COMMENTS 


: control board. 

: None 

: CalculateGlobal() in sonarmath.h. 

: LinearFitting() in sonarmath.h. 

: LinearFittingDecay() in sonarmath.h. 

: LogSonarData{) in sonarlog .h. 

: changeLEDstate() in system.h. 

: getRobotConfig() in motion.h. 

: None 

; Cleaned and documented by Mahmoud Wahdan on 3-7-95 


**★***★★•*■**★**★*★***★*■**★*********************■*■•** ***★**★******★★★•*★■**★•*★********■*•****/ 


void 

SonarSysControl(void) 

{ 


#define OVERFLOWMASK 
tdefine GROUP_MASK 
tdefine SONARJDATAO 
tdefine SONAR_DATAl 
tdefine SONAR JDATA2 
tdefine SONAR DATA3 


0x8000 

0x18 

0xfc0083f0 
0xfc0083f2 
0xfc0083f4 
0xfc0083f6 


/* overflow bit is bit 15 */ 


static int 

int 

int 

int 

int 

/* double 

CONFIGURATION 


cnt = 0; 
n; 
i; 

data[4]; 
group; 

t; */ 

current; 


/* uses only in TIMER */ 

/* is defined in definition.h */ 


tifdef TIMER 

int tick - getCount(); 

IOprintf(TimerLog, "%f ", tick / 250.0); 
tendif 


if (++cnt > 10) /* blink the tl LED */ 

{ 

cnt = 0; 

changeLEDstate(1); /* is defined in system.h */ 

) 

current = getRobotConfig(); /* getRobotConfig() is defined */ 

/* motion.h */ 
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group « ( (* (BYTE*) SONAR_CTL & GR0UPJ4ASK) » 3); 

datatO] = *(WORD*)SONAR_DATAO; 
data [1] = * (WORD*) SONAR__DATAl; 
data[2] = * (WORD*) SONAR_DATA2; 
data [3] = * (WORD*) SONAR__DATA3 ; 

for (i - 0; i < 4; i++) 

{ 

n = group_array[group][i]; 

sonar_table[n].posit.X = current.Posit.X; 
sonar__table[n] .posit.Y = current.Posit.Y; 
sonar_table[n].t = current.Theta; 
sonar_table[n],d0 = sonar_table[n].d; 

if (data[i] & OVERFLOWMASK) 

sonar_table[n].d = INFINITY; 
else 
{ 

data[i] Oxfff; /* only first 12 bits are data, */ 

/* so mask the data */ 

sonarjtable[n].d = data[i] * 0.1029; 

} 


sonar_table[n].update = 1; 


CalculateGlobal(n); 

if (sonar_table[n].fitting == 1) 
LinearFitting(n); 


/* 

is 

defined 

in 

sonarmath.c 

*/ 

/* 

is 

defined 

in 

sonarmath.c 

*/ 


if (sonar_table[n].decayFitting == 1) 

LinearFittingDecay(n); /* is defined in sonarmath.c */ 

/* LogSonarData(n); */ /* this function is moved to */ 

/* CalculateGlobal() in sonarmath.c */ 

/* because interrupt affects it & some */ 

/* logging numeric digits becomes character */ 
/* done by Mahmoud wahdan Aug. 16 95 */ 


#ifdef TIMER 

t = (tick - getCount())/250.0; 
if (t < 0.0) 

t = t + 10.0; 

IOprintf(TimerLog, "%f \n", t ); 
#endif 


} 


/********★*************************************************************** 


FILENAME 

: sonarcard.c 

PURPOSE 

: contains functions for sonar. 

CONTAINS 

: InitSonarcard() 

EnableSonar() 

DisableSonar() 

DisableAllSonar() 
EnableSonarlnterrupts() 
DisableSonarlnterrupts() 

AUTHOR 

: Patrick Byrne 

DATE 

: 20 November 1993 
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COMMENTS 


: Fri 07-22-94 Updated for Sparc mmlll FEK 
: cleaned and documented on Mon 3-6-96 by Mahmoud Wahdan 




#include 
#include 
#include 
tinclude 
tinclude 
#include 
#include 
#include 


"definitions.h" 
"sonar.h" 
"system.h" 

"time.h" 
"sonarcard.h" 
<io_defs.h> 
<reg.h> 
ctraptypes.h> 


/****** Local variables **★★**/ 
/****** _ ******^ 


static char 
static char 
static int 
static char 


*command_ptr; 

*BIM_ptr; 

enabled_sonars[NUM__SONAR$];/* NUM_SONAR = 16 is defined in sonar.h */ 
enabled; 


#define 

SONAR__VECTOR 

#define 

BIM0_CTL0 

#define 

BIM0_VECT0 

#define 

IRQ2 

#define 

SONAR_CTL 

#define 

IRQ2_REG 


0xa2 

0xfc0080cl /* BIMO control register */ 

0xfc0080c9 /* BIMO vector register */ 

0x12 

0xfc0083f9 /* sonar board command/status register */ 

OxfffcOOOb 


int 

SonarSysControlWrapper(TrapType trap, struct reg_save *reg, int vector) 

{ 


SonarSysControl(); 
return IO_INTR_EXPECTED; 

} 


/****** Source Code ******/ 

/****** - ******i 


j************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 

register. 

RETURNS 

CALLS 


CALLED BY 
COMMENTS 


InitSonarcard() 

None 

It initializes enabled_sonars list and sonar board command/status 
void 

Disablelnterrupts() in system.h. 

Enablelnterrupts() in system.h. 
mk_handler() in spare.h. 

None 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 


*************************************************************************************j 


void 

InitSonarcard(void) 

{ 

int i ; 

#define SonarlntVector 0xa2 

command_ptr » (char *)SONAR_CTL; 
BIM_ptr * (char *)BIM0_CTL0; 
enabled =0; 
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for (i=0; i<NUM_SONARS ; i++) 
enabled sonars[i] = 0; 


/* Initialization */ 


Disablelnterrupts(); /* defined in system.h */ 

/* This function is not defined */ 

mk_handler(SonarIntVector, SonarSysControlWrapper); /* defined in spare.h */ 

* (BYTE*) BIM0__VECT0 « SONAR_VECTOR; 

* (BYTE*)BIM0__CTL0 - IRQ2; 

*/ 

*(BYTE*)SONAR_CTL = 8; 

*(BYTE*)IRQ2_REG « 0x7a; 

Enablelnterrupts(); /* defined in system.h */ 

/************* if***************************************************************** ****** 

FUNCTION : EnableSonar(SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It enables the sonar group that contains sonar SonarNumber, which 

causes 

: all the sonars in that group to echo-range and write data to the data 
: registers on the sonar control board. Makes the SonarNumber’th 

position 

: of the enabled_sonars array to track which sonars are enabled. 

RETURNS : void 

CALLS : DisableSonarlnterrupts() in sonarcard.h. 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

*************************************************************************************y 

void 

EnableSonar(int SonarNumber) 

{ 

DisableSonarlnterrupts(); 

enabled_sonars[SonarNumber] 

switch (SonarNumber) { 
case 0: 
case 2: 
case 5: 
case 7: 

enabled 1= 0x01; 
break; 
case 1: 
case 3: 
case 4: 
case 6: 

enabled |= 0x02; 
break; 
case 8: 
case 9: 
case 10: 
case 11: 

enabled |= 0x04; 
break; 
case 12: 
case 13: 
case 14: 
case 15: 

enabled |= 0x08; 
break; 

} /* switch */ 

EnableSonarlnterrupts(); /* defined in sonarcard.h */ 

*command_ptr = enabled; 

> 

/************************************************************************************* 

FUNCTION : DisableSonar(SonarNumber) 



/* enable interrupts and select IRQ2 
/* zeroes the command register */ 
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PARAMETERS : int SonarNumber 

PURPOSE : It removes the sonar SonarNumber from the enabled sonars list. If 


sonar 


RETURNS 

CALLS 

CALLED BY 
COMMENTS 
★ *★★*****★★*•*■*★ 


: SonarNumber is the only enabled sonar from it's group, then the group 
: is disabled as well and will stop echo ranging. This has benefit of 
: shortening the ping interval for groups that remain enabled. 

: void 

: DisableSonarlnterrupts() in sonarcard.h 
: EnableSonarlnterrupts() in sonarcard.h. 

: DisableAllSonar() in sonarcard.h. 

: Cleaned and documented by Mahmoud Wahdan on 3-7-95 
*********★*****★*★★**********★***★*★**********************★********★**/ 


void 

DisableSonar(int SonarNumber) 

{ 

int c; 


DisableSonarlnterrupts(); /* defined in sonarcard.h */ 

enabled_sonars[SonarNumber] = 0; 


switch (SonarNumber) 

{ 

case 0: 
case 2: 
case 5: 
case 7: 

c = enabled_sonars[0] + enabled_sonars[2] + 
enabled_sonars[5] + enabled_sonars[7]; 
if (c == 0) 

enabled &= Oxfe; 
break; 
case Is 
case 3: 
case 4: 
case 6: 

c *= enabled_sonars[1] + enabled_sonars[3] + 
enabled__sonars [4] + enabled_sonars[6]; 
if (c == 0) 

enabled &= Oxfd; 
break; 
case 8: 
case 9: 
case 10: 
case 11: 

c = enabled_sonars[8] + enabled_sonars[9] + 
enabled_sonars[10] + enabled_sonars[11]; 
if (c 0) 

enabled &= Oxfb; 
break; 
case 12: 
case 13: 
case 14: 
case 15: 

c = enabled_sonars[12] + enabled_sonars[13] + 
enabled_sonars[14] + enabled_sonars[15]; 
if (c -= 0) 

enabled &= 0xf7; 
break; 

} /* switch */ 


*commandjptr = enabled; 

EnableSonarlnterrupts(); /* defined in sonarcard.h */ 


/********★****★★******★***********★*****************************★*******★*******★***** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 


DisableAllSonar() 

None 

removes the all sonars from the enabled_sonars list, 
void 

DisableSonar() in sonarcard.h. 
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CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

*************************************************************************************y 


void 

DisableAllSonar(void) 
{ 

int i; 


} 


for (i=0; i < 12; i++) 
DisableSonar(i); 


/* defined in sonarcard.h */ 


/★★a********************************************************************************** 

FUNCTION : EnableSonarlnterrupts() 

PARAMETERS : None 

PURPOSE : places sonar control board in interrupt driven mode. 

RETURNS : void 

CALLS : None 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

*************************************************************************************^ 


void 

EnableSonarlnterrupts(void) 

{ 

*BIM_ptr |= 0x10; /* cordeiro code has *BIM_ptr = 0x12; */ 


/************************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


DisableSonarlnterrupts() 

None 

stops interrupt generation by the sonar control board. A flag is set 

in the status register when data is ready, and it is the user's 

responsibility to poll the sonar system for the flag. 

void 

None 

None 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 


*************************************************************************************^ 


void 

DisableSonarlnterrupts(void) 

{ 

*BIM_ptr &= Oxef; /* cordeiro code has *BIM_ptr ~ 0; */ 


/*********************************************************************** 
FILENAME : sonarmath.c 

PURPOSE : Provides the main sonar functions 

CONTAINS : InitSonarmath() 

Sonar() 

SonarNew() 

Global() 

CalculateGlobal{) 

SetSonarParameters() 

EnableLinearFitting() 

DisableLinearFitting() 

LinearFitting() 

AddToSegment() * this function is local in sonarmath.c * 

GenerateSegment() 

EndSegment() 

BuildListO * this function is local in sonarmath.c * 

ResetMoments() * this function is local in sonarmath.c * 

GetLastSegment() 

GetCurrentSegment() 
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GetSegmentConfig{) 

WaitSegment() 

CorrectOdometryError() 

** Linear Fitting with Decay ** 

** Added by Dan Wells on 04-15-99 ** 
ResetMomentsDecay() 
EnableLinearFittingDecay() 
DisableLinearFittingDecay() 
LinearFittingDecay() 
AddToSegmentDecay() 

GetSegmentDecay() 

SetDecayFactor() 


AUTHOR : Patrick Byrne 

DATE : 20 November 1993 

COMMENTS : Fri 07-22-94 Updated for Sparc mmlll FEK 

: cleaned and documented on Mon 3-6-96 by Mahmoud Wahdan 
: and data structure LINE-SEG is removed and added to SEGMENT_RES. 
: and CUR_DATA is changed to SEGMENT_WORK. 

: Refer to this data structure in sonar.h 

**************************************************************************i 


#include "definitions.h" 
iinclude "stdiosys.h" 

#include "sonar.h" 

#include "math.h” 

#include "motion.h" 

#include "memsys.h" 

#include "sonarlog.h" 
tinclude "sonarmath.h" 

#include "trace.h” 
tinclude "geometry.h" 

/♦ADDED*/ 

#include "spare.h" 

#define SIZE 30 

/* #define DEBUG */ 

/* 

* the following constants are used in logging functions for debugging 

* purposes 
*/ 

#ifdef DEBUG 
#define DEBUG_FILE 
#define DEBUG_SIZE 
#de fine DEBUG_FREQUENCY 

XOhandle debuglO; 

#endif 


"debug.log" 

0x10000 

1 


/* #define LOCALIZATION */ 

/* 

* the following constants are used in logging functions for localization purpose */ 
#ifdef LOCALIZATION 

fdefine LOCALIZATION_FILE "localization.log" 

#define LOCALIZATIONDEBUG_SIZE 0x10000 

#define LOCALIZATION_FREQUENCY 1 

IOhandle localizationlO; 

#endif 
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/* Local variables */ 
/*-*/ 


static double Cl, C2; 

static SEGMENTJRES segstruct ; 


/*static int 

static int 

static SEGMENT RES 


SegListHead[NUMJ50NARS];*/ 
SegListTail[NUM_SONARS]; 
seg_list[NUM_SONARS][SIZE]; 


/* temporary storage for 
EndSegment func.*/ 

/* points to oldest segment 
array element */ 

/* points to newest segment 
array element */ 

/* segments for working 
memory */ 


static SEGMENTJffORK segment_data [NUM_SONARS] ; 


/* interim data for all 
sixteen sonars CUR_DATA 
is changed to SEGMENT JiFORK */ 


static SEGMENT JRESJ)ECAY seg_data_decay[NUM_SONARS]; /* data for sonars using 

linear fitting with 
decay factor */ 

static SEGMENT_RES *seg_end_config; /* temporary storage for 

segment being built is 
completed */ 

/* Local Prototypes */ 

/*-*/ 

void AddToSegment{int, POINT); 

void ResetMoments (int) ; 

void BuildList(SEGMENTJRES*, int); 

void AddToSegmentDecay(int, POINT); 


/* Source Code */ 
/* -*/ 


/****************************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 

CALLS 

CALLED BY 

COMMENTS 


: GetSegment(int SonarNumber, int SegmentNumber) 
: int SonarNumber, int SegmentNumber 
: Gets a particular segment 
: SEGMENTJRES 
: None 
: user.c 

: Created by Mark Merrell 20 May 1998 


******************************************************************************* 


/ 


SEGMENTJRES * 

GetSegment(int SonarNumber, int SegmentNumber) 

{ 

SEGMENTJRES Segment; 


Segment - seg_list [SonarNumber] [SegmentNumber]; 


return & Segment; 


} 


/******************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 


GetNumPoints(int SonarNumber) 
int SonarNumber 

Gets a number of points in the current segment 
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RETURNS : int 

CALLS : None 

CALLED BY : user.c 

COMMENTS : Created by Mark Merrell 14 July 1998 

★★★a ******************************************************************-*********i 

int 

GetNumPoints{int SonarNumber) 

{ 

return segment_data[SonarNumber].mOO; 


/***★*************★★***★****★**★*★★*****************★***************★*********** 
FUNCTION : GetNumSegments{int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : Returns the number of Segments 

RETURNS : int 

CALLS : None 

CALLED BY : user.c 

COMMENTS : Created by Mark Merrell 20 May 1998 

int 

GetNumSegments(int SonarNumber) 

{ 

return SegListTail[SonarNumber]; 

} 

FUNCTION : InitSonarmath() 

PARAMETERS : None 

PURPOSE : It resets the accumulative values mOO, mlO, mOl, m20, mil, 

m02 to zero. 

RETURNS : void 

CALLS : memsetO in memsys.h. 

: ResetMoments() in sonarmath.c *Local Prototype's in sonarmath.c* 
: ResetMomentsDecay() in sonarmath.c *Local Prototype's in 

sonarmath.c* 

CALLED BY : InitSonarO in sonar.h. 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

******************************************************************************* j 

void 

InitSonarmath(void) 

{ 

int i; 

for (i = 0; i < NUM_SONARS; i++) 

{ 

ResetMoments(i); 

ResetMomentsDecay(i); 

SegListTail[i] = -1; 


segment_data[i].seg.start.X = 0.0 
segment_data[i).seg.start.Y = 0.0 
segment__data[ij .seg.end.X = 0.0 
segment_data[i].seg.end.Y - 0.0 
segment_data[i].seg.alpha =0.0 
segment_data[i].seg.r = 0.0 
segment_data[i].seg.length = 0.0 
segment_data[i].seg.mOO = 0.0 


segment_data[i].seg.SonarNumber = i; 

} 


/* NUM_SONARS 16 is defined in sonar.h */ 

/* defined in sonarmath.c */ 

/* defined in sonarmath.c */ 
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} 

/******************** source Code For Sonar ***************************/ 
/* ================= */ 


/****************************************************************************** 
FUNCTION : Sonar(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It returns the distance (in centimeters) sensed by the 

: SonarNumber ultrasonic sensor. 

: If no echo is received, 

: then INFINITY (1.0$e$6) is returned. 

: If the distance is less than 10 cm, 

: then a 0 is returned. 

RETURNS : the distance (in centimeters) sensed by the SonarNumber 

: ultrasonic sensor. 

CALLS : None 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

******************************************************************************i 

double 

Sonar(int SonarNumber) 

{ 

sonar_table[SonarNumber].update = 0; 

return sonar_table[SonarNumber].d; /* sonar_table is global variable which 

is defined in sonar.c 

the data structure of sonar_table is 

SONARD which is defined in sonar.h */ 

} 

/****************************************************************************** 
FUNCTION : SonarNew(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : waits in a loop until new data ia available for sonar SonarNumber. 

RETURNS : the range value for sonar number. 

CALLS : None 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

****************************************************************************** j 

double 

SonarNew(int SonarNumber) 

{ 

while (sonar_table[SonarNumber].update == 0) 

; /* NULL statement */ 

sonar_table[SonarNumber].update = 0; 
return sonar_table[SonarNumber].d; 

} 

/****************************************************************************** 
FUNCTION : Global(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It returns a structure of type POINT containing the global x 

: and y coordinates of the position of the last sonar return. 

RETURNS : the global x and y coordinates of the position of the last 

: sonar return. 

CALLS : None 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

****************************************************************************** j 

POINT 

Global(int SonarNumber) 

{ 

return sonar_table[SonarNumber].global; 

} 
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/************★★******★****★**************************************************** 

FUNCTION : CalculateGlobal(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It calculates the global x and y coordinates for the range value 

: and robot configuration in the sonar table. The results are 
: stored in the sonar table. 

RETURNS : None 

CALLS : getRobotConfigO in motion.h. 

: LogSonarData() in sonarlog.h 

: Disablelnterrupts() in spare.c 

: EnaJolelnterrupts () in spare.c 

CALLED BY : SonarSysControl() in sonar.h. 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

: Disablelnterrupts(), Enablelnterrupts() & LogSonarData() added by 
: Mahmoud wahdan on Aug 16,95 to solve the problems of logging data 
: (some numeric digits becomes characters and some numbers becomes 
: very big or very small (wrong calculation). 
*********★*****★**★*******★***********★*★★**********★***★********★**★**★******/ 

void 

CalculateGlobal(int SonarNumber) 

{ 

double lx, ly. It, range; 
double sonarX, sonarY, sonarTheta; 
double robotX, robotY, robotTheta; 


Disablelnterrupts(); 

range = sonar__table [SonarNumber] .d; 

if (range >= INFINITYO) 

{ 

sonar_table[SonarNumber].global.X = INFINITY; /* sonar table is global variable 

*/ 

sonar_table[SonarNumber].global.Y - INFINITY; /* which is defined in sonar.c */ 

} /* the structure of sonar_table */ 

else /* is SONARD which is defined in 

*/ 

{ /* sonar.h */ 

sonarX = sonar_table[SonarNumber].SonarPosit.X; 
sonarY - sonar_table[SonarNumber].SonarPosit.Y; 
sonarTheta = sonar_table[SonarNumber).SonarTheta; 

robotX = sonar_table[SonarNumber].posit.X; 
robotY = sonar_table[SonarNumber].posit.Y; 
robotTheta = sonar_table[SonarNumber].t; 

/* vehicle compose sonar */ 

/* - */ 

/* global sonar config = global robot config o local sonar config */ 


/* - */ 

/* | lx | |robotX | |cos(robotTheta) -sin(robotTheta) 0| |sonarX | 

J ly |=|robotY |+jsin(robotTheta) cos(robotTheta) 0|*1sonarY | 

! It 1 |robotTheta| | 0 0 1| |sonarTheta! 

Ill II II | */ 

lx = robotX + (cos(robotTheta) * sonarX) - (sonarY * sin(robotTheta)); 
ly = robotY + (sin(robotTheta) * sonarX) + (sonarY * cos(robotTheta)); 
It = sonarTheta + robotTheta; 


/* vehicle compose sonar range */ 
/*-*/ 


/* describes the sensor’s configuration 
in the global coordinate system */ 

/* describes the sensor’s configuration 
in the local coordinate system */ 

/* describes the robot current configuration 
in the global coordinate system */ 
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sonar_table [SonarNuxnber] .global.X - lx + (cos(lt) * range); 
sonar_table[SonarNumber].global.Y - ly + (sin(lt) * range); 


LogSonarData(SonarNumber); 
EnableInterrupts(); 


/********************* Source Code For Linear Fitting *************************/ 
/ * ===^=========== : ============ ===== : * / 

/******************************************************************************* 
FUNCTION : SetSonarParameters(double cl, double c2) 

PARAMETERS : double cl 

double c2 

PURPOSE : It allows the user to adjust constants which control the 

linear fitting algorithm. 

cl is a multiplier to allow more lenancy for greater sonar 
ranges. c2 is an absolute value, cl and c2 are used to 
determine if an individual data point is usable for the 
algorithm. 

Default values are set in main.c to 0.02, 5.0 respectively, 
void 
None 

InitSonarO in sonar.h. 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 

******************************************************************* i 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 
*********** 


void 

SetSonarParameters(double cl, double c2) 

{ 

Cl = cl; 

C2 - c2; 

} 


/****************************************************************************** 
FUNCTION : EnableLinearFitting(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It causes the background system to gather data points from 

: sonar SonarNumber and form them into line segments as governed 

: by the linear fitting algorithm. 

: None 
: None 
: None 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 

******************************************************************************^ 


: Cleaned and documented by Mahmoud Wahdan on 3-7-95 


void 

EnableLinearFitting(int SonarNumber) 

{ 

sonar__table[SonarNumber] .fitting = 1; /* sonar_table is global variable 

which is defined in sonar.c the 
data structure of sonar_table is 
SONARD which is defined in sonar.h */ 

} 


/******************************************************************************* 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED BY 
COMMENTS 


DisableLinearFitting(int SonarNumber) 
int SonarNumber 

It causes background system to cease forming line segments for 
sonar SonarNumber. 

None 

GenerateSegment() in sonarmath.h. 

None 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 
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*************************** ****************************************************/ 


void 

DisableLinearFitting(int SonarNumber) 

{ 

GenerateSegment(SonarNumber); 
sonar_table[SonarNumber].fitting = 0; 

} 

/★a*****************************************************************-*********** 
FUNCTION : LinearFitting(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It controls the fitting of point data to straight line segments. 

: First, it tests if the new coming point is not far from the 
: fitted line. If the test is passed, the point is added to test 
: if the thinness test is passed. If it is passed, the addition is 
: finalized. If any of the tests fail, the line segment is ended 
: and a new one started. The completed line segment is stored in a 
: data structure called SEGMENT, and segments are linked together 
: in a linked list. 

RETURNS : None 

CALLS : GenerateSegment() in sonarmath.h 

: DisableInterrupts() in spare.c 

: Enablelnterrupts() in spare.c 

: AddToSegment() in sonarmath.c *Local Prototypes in sonarmath.c* 
CALLED BY : SonarSysControl() in sonar.h 

COMMENTS : Revised by Y. Kanayama,07-07-93 

: Cleaned and documented by Mahmoud Wahdan on 3-7-95 
: Disablelnterrupts() & Enablelnterrupts() added by 
: Mahmoud wahdan on Aug 16,95 to solve the problem of 
: some numbers becomes very big or very somall (wrong calculation). 

*********************************** *******************it*******if irit*it*it********^ / 

void 

LinearFitting(int SonarNumber) 

{ 

double sonar_range; 

double sonar_rangeO; 

POINT p; 

double alpha, r, delta; 

Disablelnterrupts(); 

sonar_range = sonar_table[SonarNumber].d; 
sonar__rangeO = sonar_table [SonarNumber] .d0; 

if (sonar_jrange < 9.3 M sonar_range > 409.0) 

{ 

GenerateSegment(SonarNumber); 

Enablelnterrupts(); 
return; 

} 

p = sonar_table[SonarNumber].global; 

if (fabs(sonar_range - sonar rangeO) >3.0 ) 

{ 

GenerateSegment(SonarNumber); 

AddToSegment(SonarNumber, p); 

Enablelnterrupts(); 
return; 

} 

if (segment_data[SonarNumber].rnOO < 1.5) 

{ 

AddToSegment(SonarNumber, p); 

Enablelnterrupts(); 
return; 

} 

r - segment_data[SonarNumber].seg.r; 
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alpha = segment_data[SonarNumber].seg.alpha; 

delta = fabs(r - p.X * cos(alpha) - p.Y * sin(alpha)); 

if (delta > MAXVAL(C2, Cl * sonar_range)) 
GenerateSegment(SonarNumber); 

AddToSegment(SonarNumber, p); 

Enablelnterrupts(); 
return; 


} 


/* End linear_fitting */ 


/****★**************************★************★*****★★********************★***** 


FUNCTION : AddToSegment(int SonarNumber) 

PARAMETERS : int SonarNumber 


PURPOSE 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


POINT p 

It calculates new interim data for the line segment and stores 
it in segment_data[SonarNumber]. 

It also changes the end point values to the point being added. 
None 

LinearFitting() in sonarmath.h. 

SonarSysControl{) in sonar.h. 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 


★*************************★★*********★***********************★*★**************/ 


void 

AddToSegment(int SonarNumber, POINT p) 

{ 


double mOO, mlO, mOl, m20, mil, m02; 

double alpha, r; 

double mux, muy, mm20,mmll, mm02; 

double x, y; 


mOO = segment_data[SonarNumber].mOO += 1.0; 
mlO = segment_data[SonarNumber].ml0 += p.X; 
mOl = segment_data[SonarNumber].mOl += p.Y; 
m2 0 = segment_data[SonarNumber].m20 += SQR(p.X); 
mil = segment_data[SonarNumber].mil += p.X * p.Y; 
m02 = segment_data[SonarNumber].m02 += SQR(p.Y); 

if (mOO < 1.5) 

segment_data[SonarNumber].seg.start = p; 

mux ■ mlO / mOO; 

muy = mOl / mOO; 

mm20 « m20 - SQR(mlO) / mOO; 

mmll = mil - mlO * mOl / mOO; 

mm02 = ra02 - SQR(mOl) / mOO; 

y - -2.0 * mmll; 

x = mra02 - mm20; 


if (mOO > 1.5) { 

if (y == 0.0 && x == 0,0) /* This test is put because when */ 

/* atan2(0,0) the system crashes */ 
alpha =0.0; /* July 24th 95 by mahmoud wahdan */ 

else 

alpha = atan2(y, x) / 2.0; 


r = mux * cos(alpha) + muy * sin(alpha); 

segment_data[SonarNumber].seg.alpha = alpha; 
segment_data[SonarNumber].seg.r = r; 
segment_data[SonarNumber].seg.end = p; 
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} 


} 


/******★******★******★********************************★******•***★************** 
FUNCTION : GenerateSegment(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It completes segments at the end of a data run. Necessary 

: because the linear fitting function only terminates a segment 
: based on the data and it has no way of knowing that the user 
: has stopped collecting data. 

RETURNS : None 

CALLS : EndSegmentO in sonarmath.h. 

: BuildListO in sonarmath.c *Local Prototypes in sonarmath.c* 

: ResetMoments() in sonarmath.c *Local Prototypes in sonarmath.c* 
CALLED BY : DisableLinearFitting() in sonarmath.h. 

: LinearFitting() in sonarmath.h. 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

****************************************************************************** j 

void 

GenerateSegment(int SonarNumber) 

{ 

SEGMENT_RES *seg_ptr; 


if (segment_data[SonarNumber].mOO > 10.0) 

{ 

seg_ptr = Endsegment(SonarNumber); 

BuildList(seg_ptr, SonarNumber); 

} 

ResetMoments(SonarNumber); 

> 

/★****★********★****★★****★**************************************************** 
FUNCTION : Endsegment(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It allocates memory for the segment data structure, loads the 

: correct values into it and returns a pointer to the structure. 

RETURNS : a pointer to the SEGMENT structure 

CALLS : None 

CALLED BY : None 

COMMENTS : Cleaned and documented by Mahmoud Wahdan on 3-7-95 

: and data structure LINE-SEG is removed and added to 
: SEGMENTJRES. Refer to this data structure in sonar.h 

******************************************************************************j 

SEGMENT_RES * 

EndSegment(int SonarNumber) 

{ 

SEGMENT_RES tmpSeg; 

SEGMENT_RES *seg_ptr; 

double length; 

double delta; 

seg__ptr = &segstruct; 

tmpSeg = segment_data[SonarNumber].seg; 

delta - tmpSeg.start.X * cos(tmpSeg.alpha) + 

tmpSeg.start.Y * sin(tmpSeg.alpha) - tmpSeg.r; 

tmpSeg.start.X -= delta * cos(tmpSeg.alpha); 
tmpSeg.start.Y -= delta * sin(tmpSeg.alpha); 
delta = tmpSeg.end.X * cos(tmpSeg.alpha) + 

tmpSeg.end.Y * sin(tmpSeg.alpha) - tmpSeg.r; 

tmpSeg.end.X -= delta * cos(tmpSeg.alpha); 
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tmpSeg.end.Y -= delta * sin(tmpSeg.alpha); 
length = sqrt(SQR{tmpSeg.start.X - tmpSeg.end.X) + 
SQR(tmpSeg.start.Y - tmpSeg.end.Y)); 


seg_j?tr->start .X 

seg_j)tr-> start. Y 

seg_ptr->end.X 

segjptr->end.Y 

se U_ptr->alpha 

segjptr->r 

seg^tr-^ength 

seg_ptr->SonarNumber 

segjptr->mOO 


tmpSeg.start.X; 
tmpSeg.start.Y; 
tmpSeg.end.X; 
tmpSeg.end.Y; 
tmpSeg.alpha; 
tmpSeg.r; 
length; 

SonarNumber; 

segment_data [SonarNumber].mOO; 


return segjptr; 


} 


/****************************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


: BuildList(SEGMENT_RES *ptr, int SonarNumber) 

: int SonarNumber 

: SEGMENT_RES *ptr 

: It accepts a pointer to a segment data structure and a sonar 
: number, and appends the segment structure to the tail of a 
: linked list of structures for that sonar. 

: None 

; LogSonarSegmentData() in sonarlog.h. 

: GenerateSegment() in sonarmath.h. 

: Cleaned and documented by Mahmoud Wahdan on 3-7-95 


****************************************************************************** 


/ 


void 

BuildList(SEGMENT_RES *ptr, int SonarNumber) 

{ 


int next; 

next = (SegListTail[SonarNumber] < 99) ? ++SegListTail[SonarNumber] : 0; 
seg_JList [SonarNumber] [next] = *ptr; 

LogSonarSegmentData(SonarNumber, seg_list[SonarNumber][next]); 
LogSonarSegmentDataArray(SonarNumber, seg_list[SonarNumber][next]); 


/****************************************************************************** 


FUNCTION : ResetMoments(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It resets the accumulative values in segment_data[SonarNumber] 

: (mOO, mlO, mOl, m20, mil, m02) to zero. 


RETURNS 
CALLS 
CALLED BY 

COMMENTS 


None 

None 

InitSonarmath() in sonarmath.h. 

GenerateSegment() in sonarmath.h. 

Cleaned and documented by Mahmoud Wahdan on 3-7-95 


****************************************************************************** 


/ 


void 

ResetMoments(int SonarNumber) 

{ 

segment_data[SonarNumber] .mOO = 0.0 
segment_data[SonarNumber].ml0 = 0.0 
segment_data[SonarNumber].mOl = 0.0 
segment_data[SonarNumber].m20 = 0.0 
segment_data[SonarNumber].mil =0.0 
segment_data[SonarNumber].m02 =0.0 

} 


y******************************************************************************* 
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FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 

CALLS 
CALLED BY 
COMMENTS 


: GetLastSegment(int SonarNumber) 

: int SonarNumber 

: It returns a pointer to the latest segment on the linked list 
: of segments for sonar. 

: If GetLastSegment is called on an empty list a null pointer is 
: returned. 

: returns the pointer to the last segment on the linked list of 
: segments for sonar. NULL if there is empty list 
: None 
: None 

: Written by Mahmoud Wahdan on 7-3-95. 




SEGMENT_RES * 

GetLastSegment(int SonarNumber) 

{ 

SEGMENT_RES *Last_seg; 
int index; 


index = SegListTail[SonarNumber]; 
if (index == -1) 


Last_seg = NULL; 


else 

Last_seg = &seg_list[SonarNumber][index]; 
return Last_seg; 


/★*****•*•*★***•*•** 


********★*******★*★*****★★**★*★****★*★★*★★**★**••*•***■*•*•★*★•*■**★*** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


GetCurrentSegment(int SonarNumber) 
int SonarNumber 

It returns a pointer to the segment currently under construction 
if there is one, otherwise, returns null pointer. This is 
accomplished by calling EndSegment, copying the data into 
segstruct and then returning a pointer to segstruct. The memory 
allocated by EndSegment is then freed. 

returns the pointer to the current segment under construction. 

None 

None 

Written by Mahmoud Wahdan on 7-3-95. 




SEGMENT__RES * 

GetCurrentSegment(int SonarNumber) 

{ 

SEGMENT_R£S *segj?tr; 

seg_ptr - EndSegment(SonarNumber); 
return seg_ptr; 


} 

/********★********★*** source Code For Localization Correction 

/* === _ ==========:=== _ ==== ^ ==:=== __ ======:=== 


*/ 


/★*********★*★*★***★*★★**★*****★★******★*****★****★•*•****★***★***** ************** 
FUNCTION : GetSegmentConfig() 

PARAMETERS : None 

PURPOSE : It returns a configuration after applied the LF. It is used in 

the localization correction. 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


returns a configuration. 

None 

None 

Written by Mahmoud Wahdan on 10-19-95. 




CONFIGURATION 
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GetSegmentConfig() 


CONFIGURATION qsegment; 

double x, y; 

Disablelnterrupts{); 

qsegment.Posit.X - seg_end_config->end.X; 
qsegment.Posit.Y = seg_end_config->end.Y; 

x = seg__end_config->start.X - seg_end__config->end.X; 

y = seg_end_config->start.Y - seg_end_config->end. Y; 


if (y == 0.0 && x == 0.0) 


qsegment.Theta = 0.0; 


else 


qsegment.Theta = atan2(y, x); 

qsegment.Kappa = 0.0; 
Enablelnterrupts(); 

return qsegment; 


} 


/****************************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED BY 
COMMENTS 


: WaitSegment(int SonarNumber) 

: int SonarNumber 

: busy wait until the line segment being built is completed. 

It is used in localization correction. 

: void 
: None 
: None 

: Written by Mahmoud Wahdan on 10-19-95. 


*******************************************************************************y 

void 


WaitSegment(int SonarNumber) 


int index; 

SEGMENT_RES *seg; 

index - SegListTail[SonarNumber]; 


while(index =- SegListTail[SonarNumber]) { 

7 

> 

seg = &seg_list[SonarNumber][SegListTail[SonarNumber]]; 
seg_end_config « &seg_list[SonarNumber][SegListTail[SonarNumber]]; 


> 


/**★****************************★*********************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 

CALLS 


Match() 

CONFIGURATION : qnew_seg 
CONFIGURATION : qmodel_seg 

compare between new segment and model wall segment, 
if match then call localization correction routine. 
It is used in localization correction, 
void 

WaitSegment() in sonarmath.c 
setRobotConfigO in seqcmd.c 
compose() in geometry.c 
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in geometry.c 


: inverse() 

CALLED BY : none 

COMMENTS : written by Mahmoud Wahdan on Nov. 4, 95 

*******************************************************************************f 
/* 

Match(CONFIGURATION qnew_seq, CONFIGURATION qmodel_seq) 

double length = 50.0; 
double orintation = 2.0; 
double delta_dist; 
double delta_theta; 

Disablelnterrupts(); 

delta_dist = eu_dis(qnew_seq.Posit.X, qnew_seq.Posit.Y, 

qmodel_seq.Posit.X, qmodel_seq.Posit.Y); 
delta_dist = sqrt((qnew_seq.Posit.X - qmodel_seq.Posit.X) * 

(qnew_seq.Posit.X - qmodel_seq.Posit.X) + 

(qnew_seq.Posit.Y - qmodel_seq.Posit.Y) * 

(qnew_seq.Posit.Y - qmodel_seq.Posit.Y)); 

delta_theta = (qnew_seq.Theta - (qmodel_seq.Theta/RAD)); 

if ((delta_dist < length/2.0) && (delta__theta < (orintation/RAD))) 
return -1; 

else 

return 0; 

} 

*/ 

/**★**************************************************************************** 
FUNCTION : CorrectOdometryError() 

PARAMETERS : CONFIGURATION : qsonar 

: CONFIGURATION : qmodel 

PURPOSE : correct the error if there is a difference between where 

: the vehicle thinks it is and where the vehicle really is. 
RETURNS : void 

CALLS : getRobotConfig() in motion.c 

: setRobotConfig() in seqcmd.c 

: compose() in geometry.c 

: inverse() in geometry.c 

CALLED BY : none 

COMMENTS ; written by Mahmoud Wahdan on Jul. 30, 95 

****************************************************^* + * + + * + ##<A .^ + A> ^^^^^^^ ++ ^^^^. 

/* 

void 

CorrectOdometryError(CONFIGURATION qsegment, CONFIGURATION qmodel) 

CONFIGURATION qodometry; 

CONFIGURATION qsegment_inverse; 

CONFIGURATION qactual; 

CONFIGURATION epsilon; 

*/ 

/***************************************************** 

/* qacual = qmodel o qsegment__inverse o qodometry */ 

/* = epsilon o qodometry */ 

/* epsilon = qmodel o qsegment_inverse */ 

/******************************************************. , 

I* 

qodometry = getRobotConfig(); 

qsegment_inverse = inverse(qsegment); 

epsilon = compose(fiqmodel, &qsegment_inverse); 

qactual - compose(&epsilon, &qodometry); 

setRobotConfiglmm(qactual); 

#ifdef LOCALIZATION 

IOprintf(localizationlO,"%f %f %f\n\n%f %f %f\n\n%f %f %f\n\n%f %f %f\n\n", 
epsilon.Posit.X, 
epsilon.Posit.Y, 
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epsilon.Theta, 

qsegment.Posit.X, 
qsegment.Posit.Y, 
qsegment.Theta, 

qodometry.Posit.X, 
qodometry.Posit.Y, 
qodome t ry.Theta, 

qactual.Posit.X, 
qactual.Posit.Y, 
qactual.Theta 
); 


#endif 

} 

*/ 


/******************* source Code For Linear Fitting with Decay ****************/ 
/* ========“=======^======^:=======-======== */ 


/***★*★************************************************************************ 


FUNCTION 

PARAMETERS 

PURPOSE 


RETURNS 
CALLS 
CALLED BY 
COMMENTS 


EnableLinearFittingDecay(int SonarNumber) 
int SonarNumber 

It causes the background system to gather data points from 
sonar SonarNumber and form them into a continuously updated 
line segments as governed by the linear fitting, with 
decay algorithm. 

None 

None 

None 

Added by Dan Wells on 04-15-99 


ft**'**'**'*********-*****************-*-******************************'A*******-****** 


/ 


void 

EnableLinearFittingDecay(int SonarNumber) 

{ 

sonar_table[SonarNumber].decayFitting = 1; /* sonar_table is global variable 

which is defined in sonar.c. the 
data structure of sonar_table is 
SONARD which is defined in sonar.h */ 


/*****★********★**************************************************************** 


FUNCTION 

PARAMETERS 

PURPOSE 

RETURNS 
CALLS 
CALLED BY 
COMMENTS 


: DisableLinearFittingDecay(int SonarNumber) 

: int SonarNumber 

: It causes background system to cease linear fitting with decay 
: for sonar SonarNumber. 

: None 
: None 
: None 

: Added by Dan Wells on 04-15-99 


*******************************************************************************j 


void 

DisableLinearFittingDecay(int SonarNumber) 

{ 

sonar_table[SonarNumber].decayFitting = 0; 

> 


/***********★**********★******************************************************* 
FUNCTION : ResetMomentsDecay(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It resets the values in seg_data_decay[SonarNumber]. 

: Effectively restarts the linear fitting with decay algorithm. 
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: A static variable is used to test for the initial call and 
: if so, sets the decay factor to the default, otherwise 
: leaves this field unchanged. This allows the user to 
: set the decay factor through the SetDecayFactor() function. - 
RETURNS : None 

CALLS : None 

CALLED BY : InitSonarmath() in sonarmath.h. 

COMMENTS : Added by Dan Wells on 04-15-99. 

★****★★***★**★★***************************************************************/ 

void 

ResetMomentsDecay(int SonarNumber) 

{ 

static int firstTimeCalled = 1; 

seg_data_decay[SonarNumber].SonarNumber - SonarNumber; 
seg_data_decay[SonarNumber].usable = 0; 
seg_data_decay[SonarNumber].numPoints = 0.0; 
seg_data_decay[SonarNumber].alpha = PI/2.0; 
seg_data_decay[SonarNumber].r = 0.0; 
if (firstTimeCalled) { 

segjdata_decay[SonarNumber].decayFactor * DEFAULTJDECAY; 
firstTimeCalled = 0; 

} 

seg_data_decay[SonarNumber].m00 = 0.0; 
seg_data_decay[SonarNumber].ml 0 = 0.0; 
seg_data_decay[SonarNumber].rnOl = 0.0; 
seg_data_decay[SonarNumber].mil = 0.0; 
seg_data_decay[SonarNumber].m20 - 0.0; 
seg__data_decay[SonarNumber] .m02 - 0.0; 


/****★***********************★★*★★***★*******★**★**★***★★★★*★■**★*★**•****★*★*★** 
FUNCTION : LinearFittingDecay(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It controls the fitting of point data to the current linear 

: fitted line using a decay factor for previously entered 
: points. 

RETURNS : None 

CALLS ; Disablelnterrupts0 in spare.c 

: Enablelnterrupts() in spare.c 

: AddToSegmentDecay() in sonarmath.c *Local Prototypes in sonarmath.c* 
CALLED BY : SonarSysControl() in sonar.h 

COMMENTS : Added by Dan Wells on 04-15-99 

: Disablelnterrupts() & Enablelnterrupts() added to solve the 
: problem of some numbers becomes very big or very 
: small (wrong calculation). 

★★★★★★a***********************************************************************/ 

void 

LinearFittingDecay(int SonarNumber) 

{ 

double sonar_range; 

double sonar_range0; 

POINT p; 

Disablelnterrupts(); 

sonar_range - sonar_table[SonarNumber].d; 
sonar_range0 = sonar_table[SonarNumber],d0; 

if (sonar_range < 9.3 || sonar_range > 80) /****409.0)*****/ 

{ 

Enablelnterrupts(); 
return; 

} 

p = sonar_table[SonarNumber].global; 

AddToSegmentDecay(SonarNumber, p); 


146 





Enablelnterrupts(); 
return; 

} 

/****************************************************************************** 
FUNCTION : AddToSegmentDecay{int SonarNumber, POINT p) 

PARAMETERS : int SonarNumber 

: POINT p 

PURPOSE : It calculates new line segment using linear fitting with decay. 

RETURNS : None 

CALLS : None 

CALLED BY : LinearFittingDecay() in sonarmath.h. 

COMMENTS •: Added by Dan Wells 04-15-99. 

******************************************************************************y 
void , 

AddToSegmentDecay(int SonarNumber, POINT p) 

{ 

double decay = seg_data_decay[SonarNumber].decayFaCtor; 

double mOO, mlO, mOl, m20, mil, m02; 

double alpha, r; 

double mux, muy, M20,Mil, M02; 

double x, y; 


mOO = seg__data_decay[SonarNumber].mOO; 
mlO = seg_data_decay[SonarNumber].ml0; 
mOl *= seg_data_decay[SonarNumber] .mOl; 
m20 = seg_data_decay[SonarNumber].m20; 
mil = seg_data__decay [SonarNumber].mil; 
m02 * seg_data_decay[SonarNumber].m02; 

mOO = mOO * decay + 1.0; 

mlO = mlO * decay + p.X; 

mOl = mOl * decay + p.Y; 

mil « mil * decay + p.X * p.Y; 

m20 = m20 * decay + p.X * p.X; 

m02 = m02 * decay + p.Y * p.Y; 

mux = mlO / mOO; 

muy * mOl / mOO; 

Mil - mil - (mlO * mOl) / mOO; 

M20 = m20 - SQR(mlO) / mOO; 

M02 = m02 - SQR(mOl) / mOO; 

seg_data_decay[SonarNumber].mOO = mOO; 
seg_data_decay[SonarNumber].mlO = mlO; 
seg_data_de cay [SonarNumber] .mOl = mOl; 
seg_data_decay[SonarNumber].mil = mil; 
seg_data_decay[SonarNumber].m20 = m20; 
seg_data_decay[SonarNumber].m02 = m02; 

seg_data_decay[SonarNumber].numPoints += 1.0; 

if (seg__data_decay[SonarNumber] .numPoints > 1.5) { /* Need 2 data */ 

/* for a line */ 

seg_data_decay[SonarNumber].usable ~ 1; 
x = M02 - M20; 
y = -2.0 * Mil; 

if ((x == 0.0) && (y == 0.0)) { 

alpha = 0.0; 

} 

else { 

alpha a atan2(y, x) / 2.0; 

} 

r = mux * cos(alpha) + muy * sin(alpha); 

seg_data_decay[SonarNumber] .alpha == alpha; 
seg_data_decay[SonarNumber].r = r; 
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} 


/**★******************************★******★**★*★********************************* 
FUNCTION : GetSegmentDecay(int SonarNumber) 

PARAMETERS : int SonarNumber 

PURPOSE : It returns a pointer to linear fitted with decay segement 

: for the specified sonar. 

RETURNS : returns the pointer 

CALLS : None 

CALLED BY : None 

COMMENTS : Added by Dan Wells on 04-15-99 

★*******★★**★*★★** ★*★*******★*★*****************************.**.**.*.*.*****.***.***,*.* j 

SEGMENT_RES_DECAY 
GetSegmentDecay(int SonarNumber) 

{ 

' SEGMENTJRES_DECAY tempSegDecay; 

tempSegDecay - seg_data_decay[SonarNumber]; 
return tempSegDecay; 


/★I****************************************************-***********.**.**.*.*******.*** 

FUNCTION : SetDecayFactor(int SonarNumber, double decay) 

PARAMETERS : int SonarNumber 

: double decay 

PURPOSE : It sets the decay factor for linear fitted with decay 

: algorithm for the specified sonar. It checks decay value 
; between 0 and 1 and sets to default if out of bounds. 

RETURNS : void 

CALLS : None 

CALLED BY : None 

COMMENTS : Added by Dan Wells on 04-22-99 

I'******************************************'*********-*************************** j 

void 

SetDecayFactor(int SonarNumber, double decay) 

{ 

if ( (decay < 0.0) |j (decay > 1.0) ) { 
printf( n \nDecay set to default”); 
decay = DEFAULT JDECAY; 

} 

seg_data_decay[SonarNumber].decayFactor - decay; 

} 
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APPENDIX B: USER FILE AND NEW WALL COMMAND FILE 


S**’*********'*'*'***************************************************************** 
File name: user.c 

Descriptions: set up system with initial parameters and motion mode, then 
check for end of run conditions 
Revision history: 

******************************************************************************/ 

tinclude "user.h" 

#include "seqcmd.h" 

#include "wallcmd.h" 
tinclude "math.h" 

#include "constants.h" 

#include "queue.h" 
finclude "geometry.h" 

I********************************************************************* ******** 
Function : user() 

Purpose : Makes a complete circuit (returns to start point) 

: using trackWall motion mode 
Parameters: void 
Returns : void 
Comments : Dan Wells 5/99 

*********************************************************************** if * itiriti ' ie I 

void user() 

{ 

CONFIGURATION startConfig, currentConfig; 
int sonarNum, wallSide; 
double sigma, decay, offset; 
int done, logSonar; 

/* Get user input and set appropriate parameters */ 

printf("\nEnter 1 to log sonar data, 0 otherwise : n ); 
logSonar = Getlnt(); 

printf("\nlnput desired offset from center of robot to wall (cm) : "); 
offset = GetRealO; 

printf("\nlnput desired smoothness (cm) e.g. 20 : "); 
sigma - GetReal{); 

setSigmalmm(sigma); 

printf("\nlnput decay factor (e.g. 0.9) : "); 
decay - GetReal() ; 

printf("\nEnter 0 for wall on RIGHT side"); 
printf("\nEnter 1 for wall on LEFT side"); 
printf("\nChoice> "); 
wallSide * Getlnt(); 

/* Turn on motion logging */ 

MotionLog(0,50,0); 

/* Set up sonar number depending on wall following side */ 
switch (wallSide) { 
case 0: 

sonarNum = S090; 
break; 
case 1: 

sonarNum = S270; 
break; 
default: 

printf("\nERROR: Illegal wall selection in user.c"); 
exit(1); 
break; 
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} 


/* Set the linear fitting decay factor for appropriate sonar */ 
SetDecayFactor(sonarNum, decay); 

/* Turn on sonar logging if user requested */ 
if (logSonar == 1) { 

InitSonarlog(); 

SonarLog(10, 0, sonarNum, SONAR_GLOBAL); 

> 

/* This sets up the wall following motion mode */ 
trackWall(sonarNum, offset); 

/* Save starting configuration for later comparison */ 
startConfig = getRobotConfigO; 

/* Let robot start to move (lets it get outside the 100 cm */ 

/* of the starting point) */ 
while (getTotalDistanceImm() < 100) { 
waitMS (50); 

/* do nothing */ 

> 

/* Now check if robot has halted or gone around the circuit */ 

/* Checking the halt condition allows data download if robot */ 

/* halts before complete circuit is made */ 
done = 0; 

while ( (VelocityLinear() >0.0) && (!done) ) { 
waitMS (50); 

currentConfig = getRobotConfigO; 

/* stop if within 100 cm and 30 degrees of starting position */ 
if ( (distance(startConfig.Posit,currentConfig.Posit) < 100) && 

(fabs(norm(startConfig.Theta - currentConfig.Theta)) < 30*d2r) ) { 
done = 1; 

} 

} 

/* Disable everything */ 

DisableLinearFittingDecay(sonarNum); 

DisableSonar(sonarNum); 

DisableSonar(S000); 

/* Stop robot */ 
stopImmO ; 

return; /* Returns to main for data download */ 


/★♦★★★a************************************************************************ 

File name: wallcmd.c 

Descriptions: set up the currentPath variable in motion.c for wall following 
mode 

Revision history: 


#include 
tinclude 
#include 
#include 
#include 
#include 
finclude 
#include 


"definitions.h" 
"motion.h" 
"stdiosys.h" 
<math.h> 
"geometry.h" 
"sonar,h" 
"sonarcard.h" 
"sonarmath.h" 


150 



/****************************************************************************** 
FUNCTION: trackWall() 

PARAMETERS: sonarNum - specifies left/right wall by sonar number S270/S090 
offset - specifies offset distance magnitude from wall 
PURPOSE: sets up the currentPath LINE variable in motion.c for wall 

following 
RETURNS: void 

CALLED BY: user() 

CALLS: setCurrentPath(), setRobotConfiglmm() 

COMMENTS: 

******************************************************************************j 

void 

trackWall(int sonarNum, double offset) 

{ 

double k; 

LINE pathElement; 

double sigma = sigmaValue{); 

/* sonarNum determines side of wall following */ 

/* Constants defined in sonar.h */ 

/* S090 - wall on right side, S270 - wall on left side */ 
switch (sonarNum) { 
case S090: 

pathElement.Type = TRACKRWALLMODE; 

offset - offset < fabs(sonar_table[S090].SonarPosit.Y)+MIN_EDGE OFFSET ? 
DEFAULT_WALL_OFFSET : offset; 

break; 
case S270: 

pathElement.Type = TRACKLWALLMODE; 

offset - offset < fabs(sonar_table[S270].SonarPosit.Y)+MIN_EDGE OFFSET ? 
-DEFAULT_WALL_OFFSET : -offset; 

break; 

default: 

printf("\nERROR: illegal value for sonarNum in trackWall()"); 

exit(1); 

break; 

} 


pathElement.config = defineConfig(0.0, 0.0, 0.0, 0.0); 

k = 1.0/sigma ; 
pathElement.a = 3.0*k; 
pathElement.b - 3.0*k*k; 
pathElement.c = k*k*k; 

pathElement.Radius - offset; /* won't initially use Radius in wall following 

so we use it to pass in the requested 
wall offset */ 

pathElement.Center.X = 0.0; 
pathElement.Center.Y = 0.0; 

setPathElement(pathElement); /* defined in motion.c - sets currentPath */ 
setRobotConfiglmm(pathElement.config); /* matches robot with currentPath */ 

EnableSonar(S000); 

EnableSonar(sonarNum) ; 

EnableLinearFittingDecay(sonarNum); 

return; 

} 
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APPENDIX C: THE WALLHUGRULE FUNCTION 
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/***★* ****************** ****************************************************** 
Function : wallHugRule() 

Purpose : To determine the linear and rotational velocities needed to put 
or keep Yamabico at the correct offset from the wall. 

Parameters: VELOCITY actual, commanded 

Returns : The required linear velocity, rotational velocity 
Comments: 

***************************************************************************** j 

static VELOCITY 

wallHugRule(VELOCITY actual, VELOCITY commanded) 

{ 

CONFIGURATION curWall, offsetConfig, relativeConfig, inverseConfig, 
tempConfig; 

SEGMENT_RES_DECAY curSeg; 
double wallx, wally, wallTheta; 
static double prevWallTheta=0.0; 

static double initCircleTheta; 

double deltaDistance; 
double deltaAngle; 

static double savedSigma; /* holds old sigma value while in turn */ 
static int pingedOnce = 0, 
pingedTwice = 0; 

static int firstTimeCalled = 1; /*only true if first time thru wallHugRule*/ 
int sonarNum; 

double distOOO, sideDist, lastSideDist; /* sonar distances */ 

static double offset; /* offset is stored in the initially unused */ 

/* (for this mode) currentPath.Radius field */ 

/* offset < 0 for left wall following */ 

/* offset > 0 for right wall following */ 

/* flags to control motion calculation */ 

static int halted = 0; /* indicates motion halted (collision) */ 

static int inTurn = 0; /* designates when robot in a turn */ 

static int inTransition = 0; /* within transition area but still */ 

/* waiting for usable Unfit data */ 
static int turnType = 0; /* 0=inside turn, l=outside turn */ 

/* 2=on circle */ 

int trackType; /* 0=wall follow, l=track currentPath */ 

double jL; /* Steering function value */ 

double kk, kk2; /* kappa, kappa squared */ 
double k; /* steering gain parameter */ 

/* Set sonarNum, turn angle, and offset for left or right wall following */ 
/* Note: turn angle (deltaAngle) is set for inside turns, use negative */ 
/* of this value for outside turns */ 

switch (currentPath.Type) { 
case TRACKRWALLMODE: 
sonarNum = S090; 
deltaAngle = PI/2.0; 
break; 

case TRACKLWALLMODE: 
sonarNum = S270; 
deltaAngle =-PI/2.0; 
break; 
default: 

printf("\nERROR in wallHugRule()") ; 

exit(1); 

break; 

} 
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67 

68 /* offset initially stored in currentPath.Radius */ 

69 if (firstTimeCalled) { 

70 offset - currentPath.Radius; 

71 } 

72 

73 /* Get current configuation of the linear fitted (w/ decay) wall */ 

74 curSeg - GetSegmentDecay{sonarNum); 

75 

76 /* calculate the wall angle */ 

77 if (fabs(norm((curSeg.alpha - PI/2.0) - vehicle.Theta)) < PI/2.0) { 

78 wallTheta = norm(curSeg.alpha - PI/2.0); 

79 } 

80 else { 

81 wallTheta ** norm (curSeg. alpha + PI/2.0); 

82 } 

83 

84 

85 /* first check forward collision eminent */ 

86 if (halted || 

87 ((sonar_table[S000].update) && 

88 (sonar_table [SOOO] .d < COLLISION_THRESH))) { 

89 halted =1; 

90 commanded.Linear = 0.0; 

91 commanded.Rotational = 0.0; 

92 return commanded; 

93 } 

94 

95 /* ensure good side sonar history data */ 

96 if (IpingedTwice && sonar_table[sonarNum].update) { 

97 sonar_table[sonarNum].update = 0; 

98 if (pingedOnce) { 

99 pingedTwice = 1; 

100 } 

101 else { 

102 pingedOnce = 1; 

103 } 

104 } 

105 


106 /*-*/ 

107 /* Handle all phases: wall following, time to turn, in turn, */ 

108 /* in transition, return to wall following from turn */ 

109 /*-*/ 

110 


111 /* not in turn (following wall), check for turn conditions */ 

112 if (JinTurn) { 

113 

114 /* set up sonar distances used in checks */ 

115 distOOO = sonar_table[S000].d + sonar_table[S000].SonarPosit.X; 

116 sideDist = sonar_table[sonarNum].d; 

117 lastSideDist = sonar_table[sonarNum].d0; 

118 

119 /* check distance from front wall, if past neutral switching point */ 

120 /* start inside turn to new path */ 

121 if ( (sonar_table[S000].update) && 

122 (distOOO <= 3.0*INSIDE_JTURN_SIGMA*PI/2.0 + fabs(offset)) ) { 

123 inTurn = 1; 

124 turnType = 0; 

125 savedSigma = sigmaValue{); 

126 setSigmalmm(INSIDE_TURN_SIGMA) ; 

127 offsetConfig = defineConfig(distOOO-fabs(offset), 0.0, deltaAngle, 0.0) 

128 

129 /* set current path to approximate next wall offset path */ 

130 currentPath.config - compose(&vehicle, &offsetConfig); 

131 currentPath.config.Kappa =0.0; 

132 k = 1.0/sigmaValue(); 

133 currentPath.a = 3.0*k ; 

134 currentPath.b = 3.0*k*k; 

135 currentPath.c = k*k*k; 

136 

137 trackType = 1; 
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sonar_table[S000].update = 0; 
DisableSonar(sonarNum); 


/* check for an outside turn, indicated by large difference between */ 

/* current and previous side sonar measurements ★ / 

else if ( (sonar_table[sonarNum].update) && 

(pingedTwice) && 

(fabs(sideDist - lastSideDist) > OUTSIDEJTURNJTHRESH) ) { 
inTurn - 1; 

savedSigma = sigma Value (); 
setSigmalmm (OUTSIDEJTURN_SIGMA) ; 
turnType =2; 

/* make current path a circle around the corner */ 

/* The radius was changed from the offset value to */ 

/* 3/4 offset value for a little tighter turn */ 

initCircleTheta = norm(vehicle.Theta); /* save for later comparison */ 

kk - -1.0/(offset*3.0/4.0); 

kk2 = kk * kk; 

k = 1.0/sigmaValue(); 

currentPath.a - 3.0*k ; 

currentPath.b = 3.0*k*k - kk2 ; 

currentPath.c = k*k*k - 3.0*k*kk2 ; 

currentPath.config = defineConfig(vehicle.Posit.X, vehicle.Posit.Y, 

initCircleTheta, kk) ; 


currentPath.Radius = 1.0/kk; 

offsetConfig = defineConfig(0.0, -offset, 0.0, 0.0); 
tempConfig = compose(&currentPath.config, soffsetConfig); 
currentPath.Center.X * tempConfig.Posit.X; 
currentPath.Center.Y = tempConfig.Posit.Y; 

trackType - 1; 

sonar_table[sonarNum].update =0; 

DisableSonar(sonarNum); 


f* otherwise keep following the wall */ 
else { 

trackType = 0; 

} 


/* in a turn, check for transition criteria */ 
else if (linTransition) { 

/* check to see if within transition area now, reset linfit data if so */ 

/* check for inside turn, vehicle angle close to line angle */ 
if ( (turnType == 0) && 

(fabs(norm(vehicle.Theta - currentPath.config.Theta)) < 

TRANSITION ANGLE)){ 

mTransition = 1; “ 

EnableSonar(sonarNum); 

ResetMomentsDecay(sonarNum); 
pingedOnce - pingedTwice = 0; 

} 

/* for outside turn check if past the corner */ 
else if (turnType == 1) { 

inverseConfig = inverse(currentPath.config); 
relativeConfig = compose(&inverseConfig, svehicle); 

/* check if past the wall corner */ 
if (relativeConfig.Posit.X > 25) { 

inTransition = 1; 

EnableSonar(sonarNum); 

ResetMomentsDecay(sonarNum); 
pingedOnce = pingedTwice - 0; 

} 

} 

/* if still on circle, check to see if angle has changed by */ 
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/* 90 +/- 3 degrees from the last wall or the initial circle angle, */ 

/* whichever comes first */ 
else if { (turnType == 2) && 

((fabs(norm(vehicle.Theta - prevWallTheta + deltaAngle)) <= 

3*d2r) || 

(fabs(norm(vehicle.Theta - initCircleTheta + deltaAngle)) <= 

3*d2r) )) { 

/* proceed on a line straight ahead */ 

currentPath.config - defineConfig(vehicle.Posit.X, vehicle.Posit.Y, 

vehicle.Theta, 0.0); 

k = 1.0/sigmaValue(); 
currentPath.a - 3.0*k ; 
currentPath.b - 3.0*k*k; 
currentPath.c = k*k*k; 

turnType = 1; 

} 


/* keep tracking toward new path line */ 
trackType = 1; 


/* in transition area */ 
else { 

/* check to see if new linfit data is usable, return to wall following */ 
if (curSeg.usable) { 
inTransition = 0; 
inTurn = 0; 
trackType = 0; 
setSigmalmm(savedSigma) ; 


/* otherwise keep tracking toward new path */ 
else { 

trackType - 1; 

} 


switch (trackType) { 

/* follow wall represented by linear fitted sonar data */ 
case 0: 

wallx = curSeg.r * cos(curSeg.alpha); 
wally = curSeg.r * sin(curSeg.alpha); 

/* wallTheta defined earlier */ 

curWall = defineConfig(wallx, wally, wallTheta, 0.0); 


/* Update the current path the robot should be following */ 
offsetConfig = defineConfig(0.0, offset, 0.0, 0.0); 
currentPath.config = compose(ScurWall, &offsetConfig); 

/* Get steering value and update commanded velocities */ 
jL = steerByWall(vehicle, curWall, offset); 

/* save wall angle */ 

prevWallTheta=wallTheta; 

break; 

/* use normal steering function to track current path */ 
case 1: 

jL = steer(vehicle, currentPath); 
break; 

default: 

printf(”\nERR0R in wallHugRule()! Exiting..."); 

exit (1); 

break; 
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firstTimeCalled - 0; 


280 
281 

282 commanded.Linear = computeLinVelocity(actual.Linear/Commanded.Linear) 

283 

284 deltaDistance = MOTION CONTROL CYCLE * commanded.Linear; 

285 

286 kappaCommanded * vehicle.Kappa + jL * deltaDistance; 

287 commanded.Rotational - kappaCommanded * commanded.Linear; 

288 

289 return commanded; 

290 } 
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